class CollisionChecker(object):
"""
check the collision of a robot
"""
def __init__(self, robotmesh):
"""
set up the collision checker
:param robotmesh is an object of robotsim/robot.robotmesh
author: weiwei
date: 20170608
"""
self.bulletworld = BulletWorld()
self.robotmesh = robotmesh
self.counter = 0
def __isSglArmCollided(self, sglbullnodes):
"""
check the self-collision of a single arm
:param sglbullnodes: a list of bullnodes starting from arm base to end-effector
:return:
author: weiwei
date: 20170608
"""
# collisioncheck, the near three links are not examined
nlink = len(sglbullnodes)
for i in range(nlink):
for k in range(i+3, nlink):
result = self.bulletworld.contactTestPair(sglbullnodes[i], sglbullnodes[k])
if result.getNumContacts():
return True
return False
def __isSglArmBdyCollided(self, sglbullnodes, bdybullnodes):
"""
check the self-collision of a single arm
:param sglbullnodes: a list of bullnodes starting from arm base to end-effector
:param bdybullnodes: a list of bullnodes for robot body
:return:
author: weiwei
date: 20170615
"""
# collisioncheck, the near three links are not examined
nlinkarm = len(sglbullnodes)
nlinkbdy = len(bdybullnodes)
for i in range(1, nlinkarm):
for k in range(0, nlinkbdy):
result = self.bulletworld.contactTestPair(sglbullnodes[i], bdybullnodes[k])
if result.getNumContacts():
return True
return False
def __isDualArmCollided(self, sglbullnodesrgt, sglbullnodeslft):
"""
check the self-collision of a single arm
the current implementation only check hands
:param sglbullnodesrgt: a list of bullnodes starting from base to end-effector
:param sglbullnodeslft: a list of bullnodes starting from base to end-effector
:return:
author: weiwei
date: 20170608
"""
# collisioncheck, the near three links are not examined
nlinkrgt = len(sglbullnodesrgt)
nlinklft = len(sglbullnodeslft)
for i in range(nlinkrgt-1, nlinkrgt):
for k in range(nlinklft-1, nlinklft):
result = self.bulletworld.contactTestPair(sglbullnodesrgt[i], sglbullnodeslft[k])
if result.getNumContacts():
return True
return False
def isSelfCollided(self, robot):
"""
check the collision of a single arm
:param armid: 'lft' or 'rgt'
:return:
author: weiwei
date: 20170608
"""
dualmnps = self.robotmesh.genmnp_list(robot)
# single arm check
sglmnps = dualmnps[0]
sglbullnodesrgt = []
sglbullnodeslft = []
# armbase is not examined
for sglmnp in sglmnps[1:len(sglmnps)-1]:
sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen')
sglbullnodesrgt.append(sglbullnode)
# hand is multinp
sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen')
sglbullnodesrgt.append(sglbullnode)
if self.__isSglArmCollided(sglbullnodesrgt):
print "right arm self collision!"
return True
else:
sglmnps = dualmnps[1]
# armbase is not examined
for sglmnp in sglmnps[1:len(sglmnps)-1]:
sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen')
sglbullnodeslft.append(sglbullnode)
# hand is multinp
sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen')
sglbullnodeslft.append(sglbullnode)
if self.__isSglArmCollided(sglbullnodeslft):
print "left arm self collision!"
return True
# dual arm check
if self.__isDualArmCollided(sglbullnodesrgt, sglbullnodeslft):
print "left-right arm self collision!"
return True
#
# arm body check
bodymnps = dualmnps[2]
bdybullnodes = []
if bodymnps:
for bodymnp in bodymnps:
bodybullnode = cd.genCollisionMeshMultiNp(bodymnp, basenodepath=None, name='autogen')
bdybullnodes.append(bodybullnode)
if self.__isSglArmBdyCollided(sglbullnodesrgt, bdybullnodes):
print "right arm body self collision!"
return True
if self.__isSglArmBdyCollided(sglbullnodeslft, bdybullnodes):
print "left right body arm self collision!"
return True
# for bullnode in sglbullnodesrgt[-4:]:
# self.bulletworld.attachRigidBody(bullnode)
# for bullnode in sglbullnodeslft:
# self.bulletworld.attachRigidBody(bullnode)
# for bullnode in bdybullnodes:
# self.bulletworld.attachRigidBody(bullnode)
return False
def isCollided(self, robot, obstaclelist):
"""
simultaneously check self-collision and robot-obstacle collision
this function should be faster than calling isselfcollided and isobstaclecollided one after another
:param robot:
:param obstaclelist:
:return:
author: weiwei
date: 20170613
"""
print self.counter
if self.counter > 200:
self.counter = 0
self.bulletworld = BulletWorld()
gc.collect()
self.counter += 1
dualmnps = self.robotmesh.genmnp_list(robot)
sglbullnodesrgt = []
sglbullnodeslft = []
# rgt arm
# single arm check
sglmnps = dualmnps[0]
# armbase is not examined
for sglmnp in sglmnps[1:len(sglmnps)-1]:
sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen')
sglbullnodesrgt.append(sglbullnode)
# hand is multinp
sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen')
sglbullnodesrgt.append(sglbullnode)
# lft arm
sglmnps = dualmnps[1]
# armbase is not examined
for sglmnp in sglmnps[1:len(sglmnps)-1]:
sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen')
sglbullnodeslft.append(sglbullnode)
# hand is multinp
sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen')
sglbullnodeslft.append(sglbullnode)
# arm obstacle check
# only check hands
nlinkrgt = len(sglbullnodesrgt)
nlinklft = len(sglbullnodeslft)
for obstaclemnp in obstaclelist:
obstaclebullnode = cd.genCollisionMeshNp(obstaclemnp, basenodepath=None, name='autogen')
for i in range(nlinkrgt - 1, nlinkrgt):
result = self.bulletworld.contactTestPair(sglbullnodesrgt[i], obstaclebullnode)
if result.getNumContacts():
print "rgtarm-obstacle collision!"
return True
for i in range(nlinklft - 1, nlinklft):
result = self.bulletworld.contactTestPair(sglbullnodeslft[i], obstaclebullnode)
if result.getNumContacts():
print "lftarm-obstacle collision!"
return True
#
# # sgl arm check
# if self.__isSglArmCollided(sglbullnodesrgt):
# print "right arm self collision!"
# return True
# else:
# if self.__isSglArmCollided(sglbullnodeslft):
# print "left arm self collision!"
# return True
#
# # dual arm check
# if self.__isDualArmCollided(sglbullnodesrgt, sglbullnodeslft):
# print "left-right arm self collision!"
# return True
# #
# # arm body check
# bodymnps = dualmnps[2]
# bdybullnodes = []
# if bodymnps:
# for bodymnp in bodymnps:
# bodybullnode = cd.genCollisionMeshMultiNp(bodymnp, basenodepath=None, name='autogen')
# bdybullnodes.append(bodybullnode)
# if self.__isSglArmBdyCollided(sglbullnodesrgt, bdybullnodes):
# print "right arm body self collision!"
# return True
# if self.__isSglArmBdyCollided(sglbullnodeslft, bdybullnodes):
# print "left right body arm self collision!"
# return True
# for bullnode in sglbullnodesrgt:
# self.bulletworld.attachRigidBody(bullnode)
# for bullnode in sglbullnodeslft:
# self.bulletworld.attachRigidBody(bullnode)
# for bullnode in bdybullnodes:
# self.bulletworld.attachRigidBody(bullnode)
return False
class FloatingPoses(object):
"""
like freetabletopplacement and tableplacements
manipulation.floatingposes corresponds to freegrip
floatingposes doesn't take into account
the position and orientation of the object
it is "free" in position and rotation. No obstacles were considered.
In contrast, each item in regrasp.floatingposes
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 "freegrip"
"s" is attached to the end of "floatingposes"
"""
def __init__(self, objpath, gdb, handpkg, base):
"""
initialization
:param objpath: path of the object
:param base: for the loadFreeAirGrip --> genHandPairs functions (for collision detection)
author: weiwei
date: 20161215, tsukuba
"""
self.handpkg = handpkg
self.handname = handpkg.getHandName()
self.hand0 = handpkg.newHandNM(hndcolor=[1,0,0,.1])
self.hand1 = handpkg.newHandNM(hndcolor=[0,0,1,.1])
self.objtrimesh = trimesh.load_mesh(objpath)
self.objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces)
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
# mat4list is nested list, floatingposemat4 is list (the flat version of mat4lsit), they are essentially the same
self.mat4list = []
self.floatingposemat4 = []
# gridsfloatingposemat4s is self.floatingposemat4 translated to different grids
self.gridsfloatingposemat4s = []
self.icos = None
self.floatinggripids = []
self.floatinggripmat4s = []
self.floatinggripcontacts = []
self.floatinggripnormals = []
self.floatinggripjawwidth = []
self.floatinggripidfreeair = []
self.bulletworld = BulletWorld()
self.handpairList = []
self.gdb = gdb
self.__loadFreeAirGrip(base)
# for IK
self.floatinggripikfeas_rgt = []
self.floatinggripikfeas_handx_rgt = []
self.floatinggripikfeas_lft = []
self.floatinggripikfeas_handx_lft = []
# for ik-feasible pairs
self.floatinggrippairsids = []
self.floatinggrippairshndmat4s = []
self.floatinggrippairscontacts = []
self.floatinggrippairsnormals = []
self.floatinggrippairsjawwidths = []
self.floatinggrippairsidfreeairs = []
self.__genPandaRotmat4()
def __loadFreeAirGrip(self, base):
"""
load self.freegripids, 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.handpkg.getHandName())
if freeairgripdata is None:
raise ValueError("Plan the freeairgrip first!")
self.freegripids = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
def __genPandaRotmat4(self, icolevel=1, angles=[0,45,90,135,180,225,270,315]):
"""
generate panda3d rotmat4 using icospheres and rotationaangle each origin-vertex vector of the icosphere
:param icolevel, the default value 1 = 42vertices
:param angles, 8 directions by default
:return: a list of [pandarotmat4, ...] size of the inner list is size of the angles
author: weiwei
date: 20170221, tsukuba
"""
self.mat4list = []
self.icos = trimesh.creation.icosphere(icolevel)
initmat4 = self.objnp.getMat()
for vert in self.icos.vertices:
self.mat4list.append([])
self.objnp.lookAt(vert[0], vert[1], vert[2])
ytozmat4 = Mat4.rotateMat(-90, self.objnp.getMat().getRow3(0))
newobjmat4 = self.objnp.getMat()*ytozmat4
for angle in angles:
tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2))
tmppandamat4 = newobjmat4*tmppandamat4
self.mat4list[-1].append(tmppandamat4)
self.objnp.setMat(initmat4)
self.floatingposemat4 = [e for l in self.mat4list for e in l]
def genHandPairs(self, base, loadser=False):
self.handpairList = []
if loadser is False:
# hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
# hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
pairidlist = list(itertools.combinations(range(len(self.freegripids)), 2))
print("len(pairidlist), /5000+1 : " + str(len(pairidlist)) + ", " + str(len(pairidlist)/5000+1))
for i in range(100,len(pairidlist),int(len(pairidlist)/5000+1)):
# for i0, i1 in pairidlist:
i0, i1 = pairidlist[i]
print(i, len(pairidlist))
self.hand0.setMat(pandanpmat4 = self.freegriprotmats[i0])
self.hand0.setJawwidth(self.freegripjawwidth[i0])
self.hand1.setMat(pandanpmat4 = self.freegriprotmats[i1])
self.hand1.setJawwidth(self.freegripjawwidth[i1])
hndbullnodei0 = cd.genCollisionMeshMultiNp(self.hand0.handnp, base.render)
hndbullnodei1 = cd.genCollisionMeshMultiNp(self.hand1.handnp, base.render)
result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1)
if not result.getNumContacts():
self.handpairList.append([self.freegripids[i0], self.freegripids[i1]])
pickle.dump(self.handpairList, open("tmp.pickle", mode="wb"))
else:
self.handpairList = pickle.load(open("tmp.pickle", mode="rb"))
# print("self.handpairList = " + str(self.handpairList))
def genFPandGs(self, grids, icolevel=1, angles=[0,45,90,135,180,225,270,315]):
"""
genterate floating poses and their grasps, this function leverages genPandaRotmat4 and transformGrips
:param icolevel
:param angles see genPandaRotmat4
:return:
author: weiwei
date: 20170221
"""
self.gridsfloatingposemat4s = []
self.__genPandaRotmat4(icolevel, angles)
for gridposition in grids:
for posemat4 in self.floatingposemat4:
tmpposemat4 = Mat4(posemat4)
tmpposemat4.setRow(3, Vec3(gridposition[0], gridposition[1], gridposition[2]))
self.gridsfloatingposemat4s.append(tmpposemat4)
self.transformGrips()
def saveToDB(self):
"""
save the floatingposes and their grasps to the database
:return:
author: weiwei
date: 20170221
"""
sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) == 0:
# the gridsfloatingposes for the self.dbobjname is not saved
sql = "INSERT INTO floatingposes(rotmat, idobject) VALUES "
for i in range(len(self.gridsfloatingposemat4s)):
# print(i, "/", len(self.gridsfloatingposemat4s))
sql += "('%s', (SELECT idobject FROM object WHERE name LIKE '%s')), " % \
(dc.mat4ToStr(self.gridsfloatingposemat4s[i]), self.dbobjname)
sql = sql[:-2] + ";"
self.gdb.execute(sql)
sql = "SELECT * FROM floatinggrips,floatingposes,object,freeairgrip,hand WHERE \
floatinggrips.idfloatingposes = floatingposes.idfloatingposes AND \
floatingposes.idobject = object.idobject AND \
object.name LIKE '%s' AND floatinggrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = hand.idhand AND hand.name LIKE '%s'" % (self.dbobjname, self.handpkg.getHandName())
result = self.gdb.execute(sql)
if len(result) == 0:
for i in range(len(self.gridsfloatingposemat4s)):
sql = "SELECT floatingposes.idfloatingposes FROM floatingposes,object WHERE \
floatingposes.idobject = object.idobject AND \
floatingposes.rotmat LIKE '%s' AND \
object.name LIKE '%s'" % (dc.mat4ToStr(self.gridsfloatingposemat4s[i]), self.dbobjname)
result = self.gdb.execute(sql)[0]
if len(result) != 0:
idfloatingposes = result[0]
sql = "SELECT * FROM floatinggrips WHERE idfloatingposes = %d" % idfloatingposes
result = self.gdb.execute(sql)
if len(result) == 0:
if len(self.floatinggripmat4s[i]) != 0:
sql = "INSERT INTO floatinggrips(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfloatingposes, idfreeairgrip) VALUES "
for j in range(len(self.floatinggripmat4s[i])):
# print("gripposes", i, "/", len(self.gridsfloatingposemat4s))
# print( "grips", j, "/", len(self.floatinggripmat4s[i]))
cct0 = self.floatinggripcontacts[i][j][0]
cct1 = self.floatinggripcontacts[i][j][1]
cctn0 = self.floatinggripnormals[i][j][0]
cctn1 = self.floatinggripnormals[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.floatinggripmat4s[i][j]), str(self.floatinggripjawwidth[i][j]), \
idfloatingposes, self.floatinggripidfreeair[i][j])
sql = sql[:-2] + ";"
self.gdb.execute(sql)
def loadFromDB(self):
"""
load the floatingposes and their grasps from the database
:return:
author: weiwei
date: 20170227
"""
self.gridsfloatingposemat4s = []
self.floatinggripids = []
self.floatinggripmat4s = []
self.floatinggripcontacts = []
self.floatinggripnormals = []
self.floatinggripjawwidth = []
self.floatinggripidfreeair = []
sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
self.gridsfloatingposemat4s.append(dc.strToMat4(resultrow[1]))
idfloatingposes = resultrow[0]
sql = "SELECT floatinggrips.idfloatinggrips, floatinggrips.contactpoint0, \
floatinggrips.contactpoint1, floatinggrips.contactnormal0, \
floatinggrips.contactnormal1, floatinggrips.rotmat, floatinggrips.jawwidth, \
floatinggrips.idfreeairgrip FROM floatinggrips,freeairgrip,hand WHERE \
floatinggrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = hand.idhand AND floatinggrips.idfloatingposes = %d AND \
hand.name = '%s'" % (idfloatingposes, self.handpkg.getHandName())
result = self.gdb.execute(sql)
if len(result) != 0:
floatinggripids = []
floatinggripmat4s = []
floatinggripcontacts = []
floatinggripnormals = []
floatinggripjawwidths = []
floatinggripidfreeairs = []
for resultrow in result:
cct0 = dc.strToV3(resultrow[1])
cct1 = dc.strToV3(resultrow[2])
cctn0 = dc.strToV3(resultrow[3])
cctn1 = dc.strToV3(resultrow[4])
floatinggripids.append(int(resultrow[0]))
floatinggripmat4s.append(dc.strToMat4(resultrow[5]))
floatinggripcontacts.append([cct0, cct1])
floatinggripnormals.append([cctn0, cctn1])
floatinggripjawwidths.append(float(resultrow[6]))
floatinggripidfreeairs.append(int(resultrow[7]))
self.floatinggripids.append(floatinggripids)
self.floatinggripmat4s.append(floatinggripmat4s)
self.floatinggripcontacts.append(floatinggripcontacts)
self.floatinggripnormals.append(floatinggripnormals)
self.floatinggripjawwidth.append(floatinggripjawwidths)
self.floatinggripidfreeair.append(floatinggripidfreeairs)
else:
print('Plan floating grips first!')
assert(False)
else:
assert('No object found!')
def transformGrips(self):
"""
transform the freeair grips to all rotmat4 in self.gridsfloatingposemat4s
:return:
author: weiwei
date: 20170221, tsukuba
"""
self.floatinggripmat4s = []
self.floatinggripcontacts = []
self.floatinggripnormals = []
self.floatinggripjawwidth = []
self.floatinggripidfreeair = []
for icomat4 in self.gridsfloatingposemat4s:
floatinggrips = self.transformGripsOnePose(icomat4)
self.floatinggripmat4s.append(floatinggrips[0])
self.floatinggripcontacts.append(floatinggrips[1])
self.floatinggripnormals.append(floatinggrips[2])
self.floatinggripjawwidth.append(floatinggrips[3])
self.floatinggripidfreeair.append(floatinggrips[4])
def transformGripsOnePose(self, rotmat4):
"""
transform the freeair grips to one specific rotmat4
:param rotmat4:
:return: [floatinggripmat4s, floatinggripcontacts, floatinggripnormals, floatinggripjawwidths, floatinggripidfreeairs]
each element in the list is also a list
"""
floatinggripmat4s = []
floatinggripcontacts = []
floatinggripnormals = []
floatinggripjawwidths = []
floatinggripidfreeairs = []
for i, gripmat4 in enumerate(self.freegriprotmats):
floatinggripmat4 = gripmat4 * rotmat4
cct0 = rotmat4.xformPoint(self.freegripcontacts[i][0])
cct1 = rotmat4.xformPoint(self.freegripcontacts[i][1])
cctn0 = rotmat4.xformPoint(self.freegripnormals[i][0])
cctn1 = rotmat4.xformPoint(self.freegripnormals[i][1])
floatinggripjawwidth = self.freegripjawwidth[i]
floatinggripidfreeair = self.freegripids[i]
floatinggripmat4s.append(floatinggripmat4)
floatinggripcontacts.append([cct0, cct1])
floatinggripnormals.append([cctn0, cctn1])
floatinggripjawwidths.append(floatinggripjawwidth)
floatinggripidfreeairs.append(floatinggripidfreeair)
return [floatinggripmat4s, floatinggripcontacts, floatinggripnormals,
floatinggripjawwidths, floatinggripidfreeairs]
def showIcomat4s(self, nodepath):
"""
show the pandamat4s generated by genPandaRotmat4
:param nodepath, where np to repreantTo, usually base.render
:return:
author: weiwei
date: 20170221, tsukuba
"""
for i, icomat4list in enumerate(self.mat4list):
vert = self.icos.vertices*100
spos = Vec3(vert[i][0], vert[i][1], vert[i][2])
for icomat4 in icomat4list:
pg.plotAxisSelf(nodepath, spos, icomat4, length=100, thickness=3)
def updateDBwithFGPairs(self, loadser = False):
"""
compute the floatinggrippairs using freegrippairs and
save the floatinggripspairs into Database
:param loadser whether use serialized data for handpairlist
:return:
author: weiwei
date: 20170301
"""
print("updateDBwithFGPairs")
print("VOHER len(self.handpairList) = " + str(len(self.handpairList)))
if len(self.handpairList) == 0:
self.genHandPairs(base, loadser)
print("NACHER len(self.handpairList) = " + str(len(self.handpairList)))
tic = time.clock()
for fpid in range(len(self.gridsfloatingposemat4s)):
print("fpid iteration: " + str(fpid))
toc = time.clock()
print(toc-tic)
if fpid != 0:
print("remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic))
print(fpid, len(self.gridsfloatingposemat4s))
# gen correspondence between freeairgripid and index
# indfgoffa means index of floatinggrips whose freeairgripid are xxx
indfgoffa = {}
print("len(self.floatinggripidfreeair[" + str(fpid) + "]) = " + str(len(self.floatinggripidfreeair[fpid])))
# Hier koennte ich auch ueber alle freegriprotmats itertieren...
for i in range(len(self.floatinggripidfreeair[fpid])):
# print(">> self.floatinggripidfreeair[fpid] = " + str(self.floatinggripidfreeair[fpid]))
indfgoffa[self.floatinggripidfreeair[fpid][i]] = i
# print("indfgoffa[" + str(self.floatinggripidfreeair[fpid][i]) + "] = " + str(indfgoffa[self.floatinggripidfreeair[fpid][i]]))
# handidpair_indfg is the pairs using index of floatinggrips
handidpair_indfg = []
lenHL = len(self.handpairList)
countttttttt = 0
for handidpair in self.handpairList:
print("Process " + str(countttttttt) + "/" + str(lenHL))
countttttttt = countttttttt + 1
# print("len(handidpair_indfg) = " + str(len(handidpair_indfg)))
hidp0 = handidpair[0]
print("handidpair[0] = " + str(handidpair[0]))
hidp1 = handidpair[1]
print("handidpair[1] = " + str(handidpair[1]))
print("indfgoffa[handidpair[0]] = " + str(indfgoffa[hidp0]))
print("indfgoffa[handidpair[1]] = " + str(indfgoffa[hidp1]))
handidpair_indfg.append([indfgoffa[handidpair[0]], indfgoffa[handidpair[1]]])
# if handidpair_indfg[0] is right, 1 is left
sql = "INSERT IGNORE INTO floatinggripspairs VALUES (%d, %d)" % \
(self.floatinggripids[fpid][handidpair_indfg[-1][0]], self.floatinggripids[fpid][handidpair_indfg[-1][1]])
self.gdb.execute(sql)
# if 1 is right, 0 is left
sql = "INSERT IGNORE INTO floatinggripspairs VALUES (%d, %d)" % \
(self.floatinggripids[fpid][handidpair_indfg[-1][1]], self.floatinggripids[fpid][handidpair_indfg[-1][0]])
self.gdb.execute(sql)
def loadIKFeasibleFGPairsFromDB(self, robot):
"""
load the IK FeasibleFGPairs
:return:
author: weiwei
date: 20170301
"""
self.loadFromDB()
self.loadIKFromDB(robot)
idrobot = self.gdb.loadIdRobot(robot)
idarmrgt = self.gdb.loadIdArm('rgt')
idarmlft = self.gdb.loadIdArm('lft')
self.floatinggrippairsids = []
self.floatinggrippairshndmat4s = []
self.floatinggrippairscontacts = []
self.floatinggrippairsnormals = []
self.floatinggrippairsjawwidths = []
self.floatinggrippairsidfreeairs = []
for fpid in range(len(self.gridsfloatingposemat4s)):
sql = "SELECT floatingposes.idfloatingposes FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s' AND floatingposes.rotmat LIKE '%s'" % \
(self.dbobjname, dc.mat4ToStr(self.gridsfloatingposemat4s[fpid]))
result = self.gdb.execute(sql)
if len(result) != 0:
idfloatingposes = result[0][0]
floatinggrippairsids = []
floatinggrippairshndmat4s = []
floatinggrippairscontacts = []
floatinggrippairsnormals = []
floatinggrippairsjawwidths = []
floatinggrippairsidfreeairs = []
sql = "SELECT floatinggripspairs.idfloatinggrips0, floatinggripspairs.idfloatinggrips1, \
fg0.contactpoint0, fg0.contactpoint1, fg0.contactnormal0, fg0.contactnormal1, fg0.rotmat, \
fg0.jawwidth, fg0.idfreeairgrip, \
fg1.contactpoint0, fg1.contactpoint1, fg1.contactnormal0, fg1.contactnormal1, fg1.rotmat, \
fg1.jawwidth, fg1.idfreeairgrip FROM floatinggripspairs, floatinggrips fg0, floatinggrips fg1, \
ikfloatinggrips ikfg0, ikfloatinggrips ikfg1 WHERE \
floatinggripspairs.idfloatinggrips0 = fg0.idfloatinggrips AND \
floatinggripspairs.idfloatinggrips1 = fg1.idfloatinggrips AND \
fg0.idfloatingposes = %d AND fg1.idfloatingposes = %d AND \
fg0.idfloatinggrips = ikfg0.idfloatinggrips AND ikfg0.feasibility like 'True' AND ikfg0.feasibility_handx like 'True' AND \
ikfg0.idrobot = %d AND ikfg0.idarm = %d AND \
fg1.idfloatinggrips = ikfg1.idfloatinggrips AND ikfg1.feasibility like 'True' AND ikfg1.feasibility_handx like 'True' AND \
ikfg1.idrobot = %d AND ikfg1.idarm = %d" % (idfloatingposes, idfloatingposes, idrobot, idarmrgt, idrobot, idarmlft)
result = self.gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
floatinggrippairsids.append([resultrow[0], resultrow[1]])
floatinggrippairshndmat4s.append([dc.strToMat4(resultrow[6]), dc.strToMat4(resultrow[13])])
rgtcct0 = dc.strToV3(resultrow[2])
rgtcct1 = dc.strToV3(resultrow[3])
lftcct0 = dc.strToV3(resultrow[9])
lftcct1 = dc.strToV3(resultrow[10])
floatinggrippairscontacts.append([[rgtcct0, rgtcct1], [lftcct0, lftcct1]])
rgtcctn0 = dc.strToV3(resultrow[4])
rgtcctn1 = dc.strToV3(resultrow[5])
lftcctn0 = dc.strToV3(resultrow[11])
lftcctn1 = dc.strToV3(resultrow[12])
floatinggrippairsnormals.append([[rgtcctn0, rgtcctn1], [lftcctn0, lftcctn1]])
floatinggrippairsjawwidths.append([float(resultrow[7]), float(resultrow[14])])
floatinggrippairsidfreeairs.append([int(resultrow[8]), int(resultrow[15])])
self.floatinggrippairsids.append(floatinggrippairsids)
self.floatinggrippairshndmat4s.append(floatinggrippairshndmat4s)
self.floatinggrippairscontacts.append(floatinggrippairscontacts)
self.floatinggrippairsnormals.append(floatinggrippairsnormals)
self.floatinggrippairsjawwidths.append(floatinggrippairsjawwidths)
self.floatinggrippairsidfreeairs.append(floatinggrippairsidfreeairs)
# for i,pairs in enumerate(self.floatinggrippairsids):
# print(i)
# print(pairs)
def updateDBwithIK(self, robot):
"""
compute the IK feasible grasps of each pose
:return:
"""
# load retraction distances
rethandx, retworldz, retworlda, worldz = self.gdb.loadIKRet()
# load idrobot
idrobot = self.gdb.loadIdRobot(robot)
self.floatinggripikfeas_rgt = []
self.floatinggripikfeas_handx_rgt = []
self.floatinggripikfeas_lft = []
self.floatinggripikfeas_handx_lft = []
tic = time.clock()
for fpid in range(len(self.gridsfloatingposemat4s)):
toc = time.clock()
print(toc-tic)
if fpid != 0:
print("remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic))
print(fpid, len(self.gridsfloatingposemat4s))
### right hand
armname = 'rgt'
feasibility = []
feasibility_handx = []
for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]):
feasibility.append('False')
feasibility_handx.append('False')
fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0]+self.floatinggripcontacts[fpid][i][1])/2
fpgsfgrcenternp = pg.v3ToNp(fpgsfgrcenter)
fpgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
minusworldy = Vec3(0,-1,0)
if Vec3(handx).angleDeg(minusworldy) < 60:
fpgsfgrcenternp_handx = pg.v3ToNp(fpgsfgrcenter+handx*rethandx)
if robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armname) is not None:
feasibility[i] = 'True'
if robot.numik(fpgsfgrcenternp_handx, fpgsrotmat3np, armname) is not None:
feasibility_handx[i] = 'True'
self.floatinggripikfeas_rgt.append(feasibility)
self.floatinggripikfeas_handx_rgt.append(feasibility_handx)
### left hand
armname = 'lft'
feasibility = []
feasibility_handx = []
for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]):
feasibility.append('False')
feasibility_handx.append('False')
fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0]+self.floatinggripcontacts[fpid][i][1])/2
fpgsfgrcenternp = pg.v3ToNp(fpgsfgrcenter)
fpgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
plusworldy = Vec3(0,1,0)
if Vec3(handx).angleDeg(plusworldy) < 60:
fpgsfgrcenternp_handx = pg.v3ToNp(fpgsfgrcenter+handx*rethandx)
if robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armname) is not None:
feasibility[i] = 'True'
if robot.numik(fpgsfgrcenternp_handx, fpgsrotmat3np, armname) is not None:
feasibility_handx[i] = 'True'
self.floatinggripikfeas_lft.append(feasibility)
self.floatinggripikfeas_handx_lft.append(feasibility_handx)
self.saveIKtoDB(idrobot)
def saveIKtoDB(self, idrobot):
"""
saveupdated IK to DB
this function is separated from updateDBwithIK for illustration
:return:
"""
for fpid in range(len(self.gridsfloatingposemat4s)):
# right arm
idarm = self.gdb.loadIdArm('rgt')
for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
sql = "INSERT IGNORE INTO ikfloatinggrips(idrobot, idarm, idfloatinggrips, feasibility, \
feasibility_handx) VALUES (%d, %d, %d, '%s', '%s')" \
% (idrobot, idarm, idfloatinggrips, self.floatinggripikfeas_rgt[fpid][i],
self.floatinggripikfeas_handx_rgt[fpid][i])
gdb.execute(sql)
# left arm
idarm = self.gdb.loadIdArm('lft')
for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
sql = "INSERT IGNORE INTO ikfloatinggrips(idrobot, idarm, idfloatinggrips, feasibility, \
feasibility_handx) VALUES (%d, %d, %d, '%s', '%s')" \
% (idrobot, idarm, idfloatinggrips, self.floatinggripikfeas_lft[fpid][i],
self.floatinggripikfeas_handx_lft[fpid][i])
gdb.execute(sql)
def loadIKFromDB(self, robot):
"""
load the feasibility of IK from db
:param robot:
:return:
author: weiwei
date: 20170301
"""
idrobot = self.gdb.loadIdRobot(robot)
self.floatinggripikfeas_rgt = []
self.floatinggripikfeas_handx_rgt = []
self.floatinggripikfeas_lft = []
self.floatinggripikfeas_handx_lft = []
for fpid in range(len(self.gridsfloatingposemat4s)):
# right arm
feasibility = []
feasibility_handx = []
idarm = self.gdb.loadIdArm('rgt')
for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
feasibility.append('False')
feasibility_handx.append('False')
sql = "SELECT feasibility, feasibility_handx FROM ikfloatinggrips WHERE \
idrobot = %d AND idarm = %d and idfloatinggrips = %d" % (idrobot, idarm, idfloatinggrips)
result = self.gdb.execute(sql)
if len(result) != 0:
feasibility[i] = result[0][0]
feasibility_handx[i] = result[0][1]
self.floatinggripikfeas_rgt.append(feasibility)
self.floatinggripikfeas_handx_rgt.append(feasibility_handx)
# left arm
feasibility = []
feasibility_handx = []
idarm = self.gdb.loadIdArm('lft')
for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
feasibility.append('False')
feasibility_handx.append('False')
sql = "SELECT feasibility, feasibility_handx FROM ikfloatinggrips WHERE \
idrobot = %d AND idarm = %d and idfloatinggrips = %d" % (idrobot, idarm, idfloatinggrips)
result = self.gdb.execute(sql)
if len(result) != 0:
feasibility[i] = result[0][0]
feasibility_handx[i] = result[0][1]
self.floatinggripikfeas_lft.append(feasibility)
self.floatinggripikfeas_handx_lft.append(feasibility_handx)
def plotOneFPandG(self, parentnp, fpid=0):
"""
plot the objpose and grasps at a specific rotmat4
:param fpid: the index of self.floatingposemat4
:return:
author: weiwei
date: 20170221, tsukuba
"""
objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces)
objnp.setMat(self.gridsfloatingposemat4s[fpid])
objnp.reparentTo(parentnp)
for i, hndrotmat4 in enumerate(self.gridsfloatingposemat4s[fpid]):
if i == 7:
# show grasps
hand0 = self.handpkg.newHandNM(hndcolor=[0, 1, 0, 1])
hand0.setMat(pandanpmat4 = hndrotmat4)
hand0.setJawwidth(self.floatinggripjawwidth[fpid][i])
hand0.reparentTo(parentnp)
print(self.handpairList)
for handidpair in self.handpairList:
if handidpair[0] == self.floatinggripidfreeair[fpid][i]:
pairedilist = [i1 for i1 in range(len(self.floatinggripidfreeair[fpid]))
if self.floatinggripidfreeair[fpid][i1]==handidpair[1]]
print(pairedilist)
i1 = pairedilist[0]
# if self.floatinggripikfeas_lft[fpid][i1] == 'True':
hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1])
hndrotmat4 = self.floatinggripmat4s[fpid][i1]
hand1.setMat(pandanpmat4 = hndrotmat4)
hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1])
hand1.reparentTo(parentnp)
if handidpair[1] == self.floatinggripidfreeair[fpid][i]:
pairedilist = [i1 for i1 in range(len(self.floatinggripidfreeair[fpid]))
if self.floatinggripidfreeair[fpid][i1]==handidpair[0]]
print(pairedilist)
i1 = pairedilist[0]
# if self.floatinggripikfeas_lft[fpid][i1] == 'True':
hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1])
hndrotmat4 = self.floatinggripmat4s[fpid][i1]
hand1.setMat(pandanpmat4 = hndrotmat4)
hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1])
hand1.reparentTo(parentnp)
def plotOneFPandGPairs(self, parentnp, fpid=0):
"""
plot the gpairss at a specific rotmat4
:param fpid: the index of self.floatingposemat4
:return:
author: weiwei
date: 20170301, tsukuba
"""
objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces)
objnp.setMat(self.gridsfloatingposemat4s[fpid])
objnp.setColor(Vec4(.7,0.3,0,1))
objnp.reparentTo(parentnp)
print(self.floatinggrippairshndmat4s[fpid])
for i, hndrotmat4pair in enumerate(self.floatinggrippairshndmat4s[fpid]):
# if i == 9:
# show grasps
hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .5])
hand0mat4 = Mat4(hndrotmat4pair[0])
# h0row3 = hand0mat4.getRow3(3)
# h0row3[2] = h0row3[2]+i*20.0
# hand0mat4.setRow(3, h0row3[2])
hand0.setMat(pandanpmat4 = hand0mat4)
hand0.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][0])
hand0.reparentTo(parentnp)
hand1 = self.handpkg.newHandNM(hndcolor=[0, .0, 1, .5])
hand1mat4 = Mat4(hndrotmat4pair[1])
# h1row3 = hand1mat4.getRow3(3)
# h1row3[2] = h1row3[2]+i*20.0
# hand1mat4.setRow(3, h1row3[2])
hand1.setMat(pandanpmat4 = hand1mat4)
hand1.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][1])
hand1.reparentTo(parentnp)
class BCMchecker(object):
"""
BBCMchecker bullet box collision model checker
"""
def __init__(self, toggledebug=False):
self.world = BulletWorld()
self._toggledebug = toggledebug
if self._toggledebug:
bulletcolliderrender = base.render.attachNewNode(
"bulletboxcollider")
debugNode = BulletDebugNode('Debug')
debugNode.showWireframe(True)
debugNode.showConstraints(True)
debugNode.showBoundingBoxes(False)
debugNode.showNormals(False)
self._debugNP = bulletcolliderrender.attachNewNode(debugNode)
self.world.setDebugNode(self._debugNP.node())
self.worldrigidbodylist = []
self._isupdateworldadded = False
def _updateworld(self, world, task):
world.doPhysics(globalClock.getDt())
return task.cont
def isBoxBoxCollided(self, objcm0, objcm1):
"""
check if two objects objcm0 as objcm1 are in collision with each other
the two objects are in the form of collision model
the AABB boxlist will be used
type "box" is required
:param objcm0: the first object
:param objcm1: the second object
:return: boolean value showing if the two objects are in collision
author: weiwei
date: 20190313
"""
objcm0boxbullnode = genBulletCDBox(objcm0)
objcm1boxbullnode = genBulletCDBox(objcm1)
result = self.world.contactTestPair(objcm0boxbullnode,
objcm1boxbullnode)
if not result.getNumContacts():
return False
else:
return True
def isBoxBoxListCollided(self, objcm0, objcmlist=[]):
"""
check if objcm0 is in collision with a list of collisionmodels in objcmlist
each obj is in the form of a collision model
:param objcm0:
:param obcmjlist: a list of collision models
:return: boolean value showing if the object and the list are in collision
author: weiwei
date: 20190313
"""
objcm0boxbullnode = genBulletCDBox(objcm0)
objcm1boxbullnode = genBulletCDBoxList(objcmlist)
result = self.world.contactTestPair(objcm0boxbullnode,
objcm1boxbullnode)
if not result.getNumContacts():
return False
else:
return True
def isBoxListBoxListCollided(self, objcm0list=[], objcm1list=[]):
"""
check if a list of collisionmodels in objcm0list is in collision with a list of collisionmodels in objcm1list
each obj is in the form of a collision model
:param objcm0list: a list of collision models
:param obcmj1list: a list of collision models
:return: boolean value showing if the object and the list are in collision
author: weiwei
date: 20190313
"""
objcm0boxbullnode = genBulletCDBoxList(objcm0list)
objcm1boxbullnode = genBulletCDBoxList(objcm1list)
result = self.world.contactTestPair(objcm0boxbullnode,
objcm1boxbullnode)
if not result.getNumContacts():
return False
else:
return True
def showBox(self, objcm):
"""
show the AABB collision meshes of the given objects
:param objcm
author: weiwei
date: 20190313
:return:
"""
if not self._toggledebug:
print(
"Toggle debug on during defining the XCMchecker object to use showfunctions!"
)
return
if not self._isupdateworldadded:
base.taskMgr.add(self._updateworld,
"updateworld",
extraArgs=[self.world],
appendTask=True)
objcmboxbullnode = genBulletCDBox(objcm)
self.world.attach(objcmboxbullnode)
self.worldrigidbodylist.append(objcmboxbullnode)
self._debugNP.show()
def showBoxList(self, objcmlist):
"""
show the AABB collision meshes of the given objects
:param objcm0, objcm1
author: weiwei
date: 20190313
:return:
"""
if not self._toggledebug:
print(
"Toggle debug on during defining the XCMchecker object to use showfunctions!"
)
return
if not self._isupdateworldadded:
base.taskMgr.add(self._updateworld,
"updateworld",
extraArgs=[self.world],
appendTask=True)
objcmboxbullnode = genBulletCDBoxList(objcmlist)
self.world.attach(objcmboxbullnode)
self.worldrigidbodylist.append(objcmboxbullnode)
self._debugNP.show()
def unshow(self):
"""
unshow everything
author: weiwei
date: 20180621
:return:
"""
base.taskMgr.remove("updateworld")
print(self.worldrigidbodylist)
for cdnode in self.worldrigidbodylist:
self.world.remove(cdnode)
self.worldrigidbodylist = []
self._debugNP.hide()
class TwoObjAss(object):
"""
the class uses genAvailableFAGs to compute grasps for the assembly of two objects
different rotations could be saved to database
"""
def __init__(self, base, obj0path, obj0Mat4, obj1path, obj1Mat4, assDirect1to0, gdb, handpkg):
"""
initiliazation
obj0 will be manipulated by rgt hand, obj1 will be manipulated by lft hand
# see genAvailableFAGs function for the details of parameters
:param base:
:param obj0path:
:param obj0Mat4:
:param obj1path:
:param obj1Mat4:
:param assDirect1to0: the assembly direction to assemble object 1 to object 0 (with length)
:param gdb:
:param handpkg:
"""
self.base = base
self.obj0path = obj0path
self.obj0Mat4 = obj0Mat4
self.dbobj0name = os.path.splitext(os.path.basename(obj0path))[0]
self.obj1path = obj1path
self.obj1Mat4 = obj1Mat4
self.dbobj1name = os.path.splitext(os.path.basename(obj1path))[0]
self.assDirect1to0 = assDirect1to0
self.gdb = gdb
self.handpkg = handpkg
# define the free ass0grip and ass1grip
# definition: objgrips: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair]
self.obj0grips = []
self.obj1grips = []
# ico means they corresponds to ico rotmats
self.gridsfloatingposemat4s = []
# right grips for each gridsfloatingposemat4
self.icoass0gripids = []
self.icoass0gripcontacts = []
self.icoass0gripnormals = []
self.icoass0griprotmat4s = []
self.icoass0gripjawwidth = []
self.icoass0gripidfreeair = []
# left grips for each gridsfloatingposemat4
self.icoass1gripids = []
self.icoass1gripcontacts = []
self.icoass1gripnormals = []
self.icoass1griprotmat4s = []
self.icoass1gripjawwidth = []
self.icoass1gripidfreeair = []
# ik related
self.icoass0gripsik = []
self.icoass0gripsik_retassdir = []
self.icoass1gripsik = []
self.icoass1gripsik_retassdir = []
# handpairList
self.handpairList = []
self.bulletworld = BulletWorld()
# ik feasible pairs for each gridsfloatingposemat4
self.icoassgrippairsids = []
self.icoassgrippairscontacts = []
self.icoassgrippairsnormals = []
self.icoassgrippairshndmat4s = []
self.icoassgrippairsjawwidths = []
self.icoassgrippairsidfreeairs = []
def genXAssPandGs(self, grids, icolevel=1, angles=[0,45,90,135,180,225,270,315]):
"""
genterate xass poses and their grasps, this function leverages genPandaRotmat4 and transformGrips
:param icolevel
:param angles see genPandaRotmat4
:return:
author: weiwei
date: 20170221
"""
self.gridsfloatingposemat4s = []
self.__genPandaRotmat4(icolevel, angles)
for gridposition in grids:
for posemat4 in self.floatingposemat4:
tmpposemat4 = Mat4(posemat4)
tmpposemat4.setRow(3, Vec3(gridposition[0], gridposition[1], gridposition[2]))
self.gridsfloatingposemat4s.append(tmpposemat4)
if len(self.obj0grips) == 0 or len(self.obj1grips) == 0:
self.__genFreeAssGrips()
self.__transformGrips()
def saveToDB(self):
"""
:return:
"""
self.icoass0gripids = []
self.icoass1gripids = []
# save to assembly table, 0-right, 1-left
idassembly = self.gdb.loadIdAssembly(self.dbobj0name, self.obj0Mat4, self.dbobj1name, self.obj1Mat4, self.assDirect1to0)
# check if rotation already exist
sql = "SELECT * FROM assemblyx WHERE assemblyx.idassembly = %d" % idassembly
result = self.gdb.execute(sql)
if len(result) == 0:
# save rotation and grasps
# if assemblyx do not exist, assemblyxgrip0/1 dont exist
for i, poserotmat4 in enumerate(self.gridsfloatingposemat4s):
sql = "INSERT INTO assemblyx(idassembly, rotmat) VALUES (%d, '%s')" % (idassembly, dc.mat4ToStr(poserotmat4))
idassemblyx = self.gdb.execute(sql)
# object 0
self.icoass0gripids.append([])
for j, hndrotmat4 in enumerate(self.icoass0griprotmat4s[i]):
strcct0 = dc.v3ToStr(self.icoass0gripcontacts[i][j][0])
strcct1 = dc.v3ToStr(self.icoass0gripcontacts[i][j][1])
strcctn0 = dc.v3ToStr(self.icoass0gripnormals[i][j][0])
strcctn1 = dc.v3ToStr(self.icoass0gripnormals[i][j][1])
strrotmat = dc.mat4ToStr(hndrotmat4)
strjawwidth = str(self.icoass0gripjawwidth[i][j])
idfreeairgrip = self.icoass0gripidfreeair[i][j]
sql = "INSERT INTO assemblyxgrips0(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idassemblyx, idfreeairgrip) VALUES ('%s', '%s', '%s', '%s', '%s', '%s', %d, \
%d)" % (strcct0, strcct1, strcctn0, strcctn1, strrotmat, strjawwidth, idassemblyx, idfreeairgrip)
idaxg = self.gdb.execute(sql)
self.icoass0gripids[-1].append(idaxg)
# object 1
self.icoass1gripids.append([])
for j, hndrotmat4 in enumerate(self.icoass1griprotmat4s[i]):
strcct0 = dc.v3ToStr(self.icoass1gripcontacts[i][j][0])
strcct1 = dc.v3ToStr(self.icoass1gripcontacts[i][j][1])
strcctn0 = dc.v3ToStr(self.icoass1gripnormals[i][j][0])
strcctn1 = dc.v3ToStr(self.icoass1gripnormals[i][j][1])
strrotmat = dc.mat4ToStr(hndrotmat4)
strjawwidth = str(self.icoass1gripjawwidth[i][j])
idfreeairgrip = self.icoass1gripidfreeair[i][j]
sql = "INSERT INTO assemblyxgrips1(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idassemblyx, idfreeairgrip) VALUES ('%s', '%s', '%s', '%s', '%s', '%s', %d, \
%d)" % (strcct0, strcct1, strcctn0, strcctn1, strrotmat, strjawwidth, idassemblyx, idfreeairgrip)
idaxg = self.gdb.execute(sql)
self.icoass1gripids[-1].append(idaxg)
# TODO: Have to delete assemblyx when grasps are deleted
# TODO: write algorithms to enable insertion of grasps using idassemblyx
def loadFromDB(self):
"""
:return:
"""
self.gridsfloatingposemat4s = []
self.icoass0gripids = []
self.icoass0gripcontacts = []
self.icoass0gripnormals = []
self.icoass0griprotmat4s = []
self.icoass0gripjawwidth = []
self.icoass0gripidfreeair = []
self.icoass1gripids = []
self.icoass1gripcontacts = []
self.icoass1gripnormals = []
self.icoass1griprotmat4s = []
self.icoass1gripjawwidth = []
self.icoass1gripidfreeair = []
idassembly = self.gdb.loadIdAssembly(self.dbobj0name, self.obj0Mat4, self.dbobj1name, self.obj1Mat4, self.assDirect1to0)
# check if rotation already exist
sql = "SELECT assemblyx.idassemblyx, assemblyx.rotmat FROM assemblyx WHERE assemblyx.idassembly = %d" % idassembly
result = self.gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
poserotmat4 = dc.strToMat4(resultrow[1])
self.gridsfloatingposemat4s.append(poserotmat4)
idassemblyx = resultrow[0]
# g0
sql = "SELECT * FROM assemblyxgrips0 WHERE idassemblyx = %d" % idassemblyx
result = self.gdb.execute(sql)
if len(result) != 0:
self.icoass0gripids.append([])
self.icoass0gripcontacts.append([])
self.icoass0gripnormals.append([])
self.icoass0griprotmat4s.append([])
self.icoass0gripjawwidth.append([])
self.icoass0gripidfreeair.append([])
for resultrow in result:
self.icoass0gripids[-1].append(resultrow[0])
cct0 = dc.strToV3(resultrow[1])
cct1 = dc.strToV3(resultrow[2])
cctn0 = dc.strToV3(resultrow[3])
cctn1 = dc.strToV3(resultrow[4])
self.icoass0gripcontacts[-1].append([cct0, cct1])
self.icoass0gripnormals[-1].append([cctn0, cctn1])
self.icoass0griprotmat4s[-1].append(dc.strToMat4(resultrow[5]))
self.icoass0gripjawwidth[-1].append(float(resultrow[6]))
self.icoass0gripidfreeair[-1].append(int(resultrow[8]))
# g1
sql = "SELECT * FROM assemblyxgrips1 WHERE idassemblyx = %d" % idassemblyx
result = self.gdb.execute(sql)
if len(result) != 0:
self.icoass1gripids.append([])
self.icoass1gripcontacts.append([])
self.icoass1gripnormals.append([])
self.icoass1griprotmat4s.append([])
self.icoass1gripjawwidth.append([])
self.icoass1gripidfreeair.append([])
for resultrow in result:
self.icoass1gripids[-1].append(resultrow[0])
cct0 = dc.strToV3(resultrow[1])
cct1 = dc.strToV3(resultrow[2])
cctn0 = dc.strToV3(resultrow[3])
cctn1 = dc.strToV3(resultrow[4])
self.icoass1gripcontacts[-1].append([cct0, cct1])
self.icoass1gripnormals[-1].append([cctn0, cctn1])
self.icoass1griprotmat4s[-1].append(dc.strToMat4(resultrow[5]))
self.icoass1gripjawwidth[-1].append(float(resultrow[6]))
self.icoass1gripidfreeair[-1].append(int(resultrow[8]))
def updateDBwithHandPairs(self):
"""
compute the assgrippairs using assfreegrippairs and
save the assgrippairs into Database
:return:
author: weiwei
date: 20170307
"""
self.__genHandPairs(base)
tic = time.time()
for fpid in range(len(self.gridsfloatingposemat4s)):
toc = time.time()
print(toc-tic)
if fpid != 0:
print("remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic))
print(fpid, len(self.gridsfloatingposemat4s))
# gen correspondence between freeairgripid and index
# indagoffa means index of assgrips whose freeairgripid are xxx
indagoffa0 = {}
for i in range(len(self.icoass0gripidfreeair[fpid])):
indagoffa0[self.icoass0gripidfreeair[fpid][i]] = i
indagoffa1 = {}
for i in range(len(self.icoass1gripidfreeair[fpid])):
indagoffa1[self.icoass1gripidfreeair[fpid][i]] = i
# handidpair_indag is the pairs using index of floatinggrips
handidpair_indag = []
for handidpair in self.handpairList:
handidpair_indag.append([indagoffa0[handidpair[0]], indagoffa1[handidpair[1]]])
sql = "INSERT IGNORE INTO assemblyxgrippairs VALUES (%d, %d)" % \
(self.icoass0gripids[fpid][handidpair_indag[-1][0]], self.icoass1gripids[fpid][handidpair_indag[-1][1]])
self.gdb.execute(sql)
def updateDBwithIK(self, robot):
"""
compute the IK feasible grasps of each pose
:return:
"""
# load idrobot
idrobot = self.gdb.loadIdRobot(robot)
self.assgikfeas0 = []
self.assgikfeas0_retassdir = []
self.assgikfeas1= []
self.assgikfeas1_retassdir = []
tic = time.time()
for fpid, apmat in enumerate(self.gridsfloatingposemat4s):
assdir1to0 = apmat.xformVec(self.assDirect1to0)
assdir0to1 = apmat.xformVec(-self.assDirect1to0)
toc = time.time()
print(toc-tic)
if fpid != 0:
print("remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic))
print(fpid, len(self.gridsfloatingposemat4s))
### right hand # rgt = 0
armname = 'rgt'
assgikfeas0 = []
assgikfeas0_retassdir = []
for i, hndrotmat4 in enumerate(self.icoass0griprotmat4s[fpid]):
assgikfeas0.append('False')
assgikfeas0_retassdir.append('False')
assgsfgrcenter = (self.icoass0gripcontacts[fpid][i][0]+self.icoass0gripcontacts[fpid][i][1])/2
assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter)
assgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
minusworldy = Vec3(0,-1,0)
if Vec3(handx).angleDeg(minusworldy) < 60:
assgsfgrcenternp_retassdir = pg.v3ToNp(assgsfgrcenter+assdir1to0)
if robot.numik(assgsfgrcenternp, assgsrotmat3np, armname) is not None:
assgikfeas0[i] = 'True'
if robot.numik(assgsfgrcenternp_retassdir, assgsrotmat3np, armname) is not None:
assgikfeas0_retassdir[i] = 'True'
self.icoass0gripsik.append(assgikfeas0)
self.icoass0gripsik_retassdir.append(assgikfeas0_retassdir)
### left hand
armname = 'lft'
assgikfeas1 = []
assgikfeas1_retassdir = []
for i, hndrotmat4 in enumerate(self.icoass1griprotmat4s[fpid]):
assgikfeas1.append('False')
assgikfeas1_retassdir.append('False')
assgsfgrcenter = (self.icoass1gripcontacts[fpid][i][0]+self.icoass1gripcontacts[fpid][i][1])/2
assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter)
assgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
plusworldy = Vec3(0,1,0)
if Vec3(handx).angleDeg(plusworldy) < 60:
assgsfgrcenternp_retassdir = pg.v3ToNp(assgsfgrcenter+assdir0to1)
if robot.numik(assgsfgrcenternp, assgsrotmat3np, armname) is not None:
assgikfeas1[i] = 'True'
if robot.numik(assgsfgrcenternp_retassdir, assgsrotmat3np, armname) is not None:
assgikfeas1_retassdir[i] = 'True'
self.icoass1gripsik.append(assgikfeas1)
self.icoass1gripsik_retassdir.append(assgikfeas1_retassdir)
self.__saveIKtoDB(idrobot)
def loadIKFeasibleAGPairsFromDB(self, robot):
"""
load the IK FeasibleAGPairs AG -> assgrippairs
:return:
author: weiwei
date: 20170301
"""
self.loadFromDB()
self.__loadIKFromDB(robot)
idrobot = self.gdb.loadIdRobot(robot)
idarmrgt = self.gdb.loadIdArm('rgt')
idarmlft = self.gdb.loadIdArm('lft')
self.icoassgrippairsids = []
self.icoassgrippairscontacts = []
self.icoassgrippairsnormals = []
self.icoassgrippairshndmat4s = []
self.icoassgrippairsjawwidths = []
self.icoassgrippairsidfreeairs = []
idassembly = self.gdb.loadIdAssembly(self.dbobj0name, self.obj0Mat4, self.dbobj1name, self.obj1Mat4, self.assDirect1to0)
for fpid in range(len(self.gridsfloatingposemat4s)):
sql = "SELECT assemblyx.idassemblyx FROM assemblyx WHERE assemblyx.idassembly = %d AND \
assemblyx.rotmat LIKE '%s'" % (idassembly, dc.mat4ToStr(self.gridsfloatingposemat4s[fpid]))
result = self.gdb.execute(sql)
if len(result) != 0:
idfloatingposes = result[0][0]
icoassgrippairsids = []
icoassgrippairscontacts = []
icoassgrippairsnormals = []
icoassgrippairshndmat4s = []
icoassgrippairsjawwidths = []
icoassgrippairsidfreeairs = []
sql = "SELECT assemblyxgrippairs.idassemblyxgrips0, assemblyxgrippairs.idassemblyxgrips1, \
assemblyxgrips0.contactpoint0, assemblyxgrips0.contactpoint1, assemblyxgrips0.contactnormal0, \
assemblyxgrips0.contactnormal1, assemblyxgrips0.rotmat, \
assemblyxgrips0.jawwidth, assemblyxgrips0.idfreeairgrip, \
assemblyxgrips1.contactpoint0, assemblyxgrips1.contactpoint1, assemblyxgrips1.contactnormal0, \
assemblyxgrips1.contactnormal1, assemblyxgrips1.rotmat, \
assemblyxgrips1.jawwidth, assemblyxgrips1.idfreeairgrip \
FROM assemblyxgrippairs, assemblyxgrips0, assemblyxgrips1, \
ikassemblyxgrips0, ikassemblyxgrips1 WHERE \
assemblyxgrippairs.idassemblyxgrips0 = assemblyxgrips0.idassemblyxgrips0 AND \
assemblyxgrippairs.idassemblyxgrips1 = assemblyxgrips1.idassemblyxgrips1 AND \
assemblyxgrips0.idassemblyx = %d AND assemblyxgrips1.idassemblyx = %d AND \
assemblyxgrips0.idassemblyxgrips0 = ikassemblyxgrips0.idassemblyxgrips0 AND \
ikassemblyxgrips0.feasibility like 'True' AND ikassemblyxgrips0.feasibility_assdir like 'True' AND \
ikassemblyxgrips0.idrobot = %d AND ikassemblyxgrips0.idarm = %d AND \
assemblyxgrips1.idassemblyxgrips1 = ikassemblyxgrips1.idassemblyxgrips1 AND \
ikassemblyxgrips1.feasibility like 'True' AND ikassemblyxgrips1.feasibility_assdir like 'True' AND \
ikassemblyxgrips1.idrobot = %d AND ikassemblyxgrips1.idarm = %d" % \
(idfloatingposes, idfloatingposes, idrobot, idarmrgt, idrobot, idarmlft)
result = self.gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
icoassgrippairsids.append([resultrow[0], resultrow[1]])
rgtcct0 = dc.strToV3(resultrow[2])
rgtcct1 = dc.strToV3(resultrow[3])
lftcct0 = dc.strToV3(resultrow[9])
lftcct1 = dc.strToV3(resultrow[10])
icoassgrippairscontacts.append([[rgtcct0, rgtcct1], [lftcct0, lftcct1]])
rgtcctn0 = dc.strToV3(resultrow[4])
rgtcctn1 = dc.strToV3(resultrow[5])
lftcctn0 = dc.strToV3(resultrow[11])
lftcctn1 = dc.strToV3(resultrow[12])
icoassgrippairsnormals.append([[rgtcctn0, rgtcctn1], [lftcctn0, lftcctn1]])
icoassgrippairshndmat4s.append([dc.strToMat4(resultrow[6]), dc.strToMat4(resultrow[13])])
icoassgrippairsjawwidths.append([float(resultrow[7]), float(resultrow[14])])
icoassgrippairsidfreeairs.append([int(resultrow[8]), int(resultrow[15])])
self.icoassgrippairsids.append(icoassgrippairsids)
self.icoassgrippairscontacts.append(icoassgrippairscontacts)
self.icoassgrippairsnormals.append(icoassgrippairsnormals)
self.icoassgrippairshndmat4s.append(icoassgrippairshndmat4s)
self.icoassgrippairsjawwidths.append(icoassgrippairsjawwidths)
self.icoassgrippairsidfreeairs.append(icoassgrippairsidfreeairs)
def __genHandPairs(self, base):
self.handpairList = []
self.__genFreeAssGrips()
ass0gripcontacts, ass0gripnormals, ass0griprotmat4s, ass0gripjawwidth, ass0gripidfreeair = self.obj0grips
ass1gripcontacts, ass1gripnormals, ass1griprotmat4s, ass1gripjawwidth, ass1gripidfreeair = self.obj1grips
hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
i0list = range(len(ass0gripidfreeair))
i1list = range(len(ass1gripidfreeair))
pairidlist = list(itertools.product(i0list, i1list))
print(len(pairidlist)/10000+1)
for i in range(0,len(pairidlist),len(pairidlist)/10000+1):
# for i0, i1 in pairidlist:
i0, i1 = pairidlist[i]
print(i, len(pairidlist))
hand0.setMat(ass0griprotmat4s[i0])
hand0.setJawwidth(ass0gripjawwidth[i0])
hand1.setMat(ass1griprotmat4s[i1])
hand1.setJawwidth(ass1gripjawwidth[i1])
hndbullnodei0 = cd.genCollisionMeshMultiNp(hand0.handnp, base.render)
hndbullnodei1 = cd.genCollisionMeshMultiNp(hand1.handnp, base.render)
result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1)
if not result.getNumContacts():
self.handpairList.append([ass0gripidfreeair[i0], ass1gripidfreeair[i1]])
# print ass0gripidfreeair
# print self.icoass0gripidfreeair[0]
# print ass1gripidfreeair
# print self.icoass1gripidfreeair[0]
# print self.handpairList
# assert "compare"
def __saveIKtoDB(self, idrobot):
"""
saveupdated IK to DB
this function is separated from updateDBwithIK for illustration
:return:
"""
for fpid in range(len(self.gridsfloatingposemat4s)):
# right arm
idarm = self.gdb.loadIdArm('rgt')
for i, idicoass0grip in enumerate(self.icoass0gripids[fpid]):
sql = "INSERT IGNORE INTO ikassemblyxgrips0(idrobot, idarm, idassemblyxgrips0, feasibility, \
feasibility_assdir) VALUES (%d, %d, %d, '%s', '%s')" \
% (idrobot, idarm, idicoass0grip, self.icoass0gripsik[fpid][i],
self.icoass0gripsik_retassdir[fpid][i])
gdb.execute(sql)
# left arm
idarm = self.gdb.loadIdArm('lft')
for i, idicoass1grip in enumerate(self.icoass1gripids[fpid]):
sql = "INSERT IGNORE INTO ikassemblyxgrips1(idrobot, idarm, idassemblyxgrips1, feasibility, \
feasibility_assdir) VALUES (%d, %d, %d, '%s', '%s')" \
% (idrobot, idarm, idicoass1grip, self.icoass1gripsik[fpid][i],
self.icoass1gripsik_retassdir[fpid][i])
gdb.execute(sql)
def __loadIKFromDB(self, robot):
"""
load the feasibility of IK from db
:param robot:
:return:
author: weiwei
date: 20170307
"""
idrobot = self.gdb.loadIdRobot(robot)
self.assgikfeas0 = []
self.assgikfeas0_retassdir = []
self.assgikfeas1= []
self.assgikfeas1_retassdir = []
for fpid in range(len(self.gridsfloatingposemat4s)):
# right arm
assgikfeas0 = []
assgikfeas0_retassdir = []
idarm = self.gdb.loadIdArm('rgt')
for i, idassgrips in enumerate(self.icoass0gripids[fpid]):
assgikfeas0.append('False')
assgikfeas0_retassdir.append('False')
sql = "SELECT feasibility, feasibility_assdir FROM ikassemblyxgrips0 WHERE \
idrobot = %d AND idarm = %d and idassemblyxgrips0 = %d" % (idrobot, idarm, idassgrips)
result = self.gdb.execute(sql)
if len(result) != 0:
assgikfeas0[i] = result[0][0]
assgikfeas0_retassdir[i] = result[0][1]
else:
assert "Compute ik of the freeassemblygrips first!"
self.assgikfeas0.append(assgikfeas0)
self.assgikfeas0_retassdir.append(assgikfeas0_retassdir)
# left arm
assgikfeas1 = []
assgikfeas1_retassdir = []
idarm = self.gdb.loadIdArm('lft')
for i, idassgrips in enumerate(self.icoass1gripids[fpid]):
assgikfeas1.append('False')
assgikfeas1_retassdir.append('False')
sql = "SELECT feasibility, feasibility_assdir FROM ikassemblyxgrips1 WHERE \
idrobot = %d AND idarm = %d and idassemblyxgrips1 = %d" % (idrobot, idarm, idassgrips)
result = self.gdb.execute(sql)
if len(result) != 0:
assgikfeas1[i] = result[0][0]
assgikfeas1_retassdir[i] = result[0][1]
else:
assert "Compute ik of the freeassemblygrips first!"
self.assgikfeas1.append(assgikfeas1)
self.assgikfeas1_retassdir.append(assgikfeas1_retassdir)
def __genPandaRotmat4(self, icolevel=1, angles=[0, 45, 90, 135, 180, 225, 270, 315]):
"""
generate panda3d rotmat4 using icospheres and rotationaangle each origin-vertex vector of the icosphere
:param icolevel, the default value 1 = 42vertices
:param angles, 8 directions by default
:return: a list of [pandarotmat4, ...] size of the inner list is size of the angles
author: weiwei
date: 20170221, tsukuba (copied from regrasp/floatingposes.py)
"""
objnp = pg.genObjmnp(self.obj0path)
mat4list = []
self.icos = trimesh.creation.icosphere(icolevel)
initmat4 = objnp.getMat()
for vert in self.icos.vertices:
mat4list.append([])
objnp.lookAt(vert[0], vert[1], vert[2])
ytozmat4 = Mat4.rotateMat(-90, objnp.getMat().getRow3(0))
newobjmat4 = objnp.getMat() * ytozmat4
for angle in angles:
tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2))
tmppandamat4 = newobjmat4 * tmppandamat4
mat4list[-1].append(tmppandamat4)
objnp.setMat(initmat4)
self.floatingposemat4 = [e for l in mat4list for e in l]
def __genFreeAssGrips(self):
self.obj0grips = []
self.obj1grips = []
self.obj0grips, self.obj1grips = \
genAvailableFAGs(self.base, self.obj0path, self.obj0Mat4,
self.obj1path, self.obj1Mat4, self.gdb, self.handpkg)
def __transformGripsOnePose(self, objgrips, rotmat4):
"""
transform the freeair grips to one specific rotmat4
:param objgrips: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair]
:param rotmat4:
:return: [xassgripcontacts, xassgripnormals, xassgriprotmat4s, xassgripjawwidth, xassgripidfreeair]
each element in the list is also a list
author: weiwei
date: 20170307
"""
assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair = objgrips
xassgripcontacts = []
xassgripnormals = []
xassgriprotmat4s = []
xassgripjawwidth = []
xassgripidfreeair = []
for i, gripmat4 in enumerate(assgriprotmat4s):
floatinggripmat4 = gripmat4 * rotmat4
cct0 = rotmat4.xformPoint(assgripcontacts[i][0])
cct1 = rotmat4.xformPoint(assgripcontacts[i][1])
cctn0 = rotmat4.xformPoint(assgripnormals[i][0])
cctn1 = rotmat4.xformPoint(assgripnormals[i][1])
xassgripcontacts.append([cct0, cct1])
xassgripnormals.append([cctn0, cctn1])
xassgriprotmat4s.append(floatinggripmat4)
xassgripjawwidth.append(assgripjawwidth[i])
xassgripidfreeair.append(assgripidfreeair[i])
return [xassgripcontacts, xassgripnormals, xassgriprotmat4s, xassgripjawwidth, xassgripidfreeair]
def __transformGrips(self):
"""
transform the freeass grips to all rotmat4 in self.gridsfloatingposemat4s
:return:
author: weiwei
date: 20170221, tsukuba (copied from regrasp/floatingposes.py)
"""
self.icoass0gripcontacts = []
self.icoass0gripnormals = []
self.icoass0griprotmat4s = []
self.icoass0gripjawwidth = []
self.icoass0gripidfreeair = []
for icomat4 in self.gridsfloatingposemat4s:
xassgrips = self.__transformGripsOnePose(self.obj0grips, icomat4)
self.icoass0gripcontacts.append(xassgrips[0])
self.icoass0gripnormals.append(xassgrips[1])
self.icoass0griprotmat4s.append(xassgrips[2])
self.icoass0gripjawwidth.append(xassgrips[3])
self.icoass0gripidfreeair.append(xassgrips[4])
self.icoass1gripcontacts = []
self.icoass1gripnormals = []
self.icoass1griprotmat4s = []
self.icoass1gripjawwidth = []
self.icoass1gripidfreeair = []
for icomat4 in self.gridsfloatingposemat4s:
xassgrips = self.__transformGripsOnePose(self.obj1grips, icomat4)
self.icoass1gripcontacts.append(xassgrips[0])
self.icoass1gripnormals.append(xassgrips[1])
self.icoass1griprotmat4s.append(xassgrips[2])
self.icoass1gripjawwidth.append(xassgrips[3])
self.icoass1gripidfreeair.append(xassgrips[4])
class GameStatePlaying(GState):
VIDAS = 3
#LEVEL_TIME = 100
LEVEL_TIME = 50
def start(self):
self._playing_node = render.attachNewNode("playing-node")
self._playing_node2d = aspect2d.attachNewNode("playing2d-node")
self.keyMap = {"left":0, "right":0, "up":0, "down":0}
base.accept( "escape" , sys.exit)
props = WindowProperties()
props.setCursorHidden(True)
base.disableMouse()
props.setMouseMode(WindowProperties.MRelative)
base.win.requestProperties(props)
base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2)
self._modulos = None
self._paneles = None
self._num_lvl = 1
self._num_lvls = 2
self.loadLevel()
self.loadGUI()
self.loadBkg()
self._last_t = None
self._last_t_space = 0
def stop(self):
self._playing_node.removeNode()
self._playing_node2d.removeNode()
def loadLevel(self):
self._level_time = self.LEVEL_TIME
self.initBullet()
self._player = Player(self.world)
self.loadMap()
self.setAI()
def initBullet(self):
self.world = BulletWorld()
#self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setGravity(Vec3(0, 0, -1.62))
groundShape = BulletPlaneShape(Vec3(0, 0, 1), 0)
groundNode = BulletRigidBodyNode('Ground')
groundNode.addShape(groundShape)
self.world.attachRigidBody(groundNode)
def loadBkg(self):
self.environ1 = loader.loadModel("../data/models/skydome")
self.environ1.reparentTo(self._playing_node)
self.environ1.setPos(0,0,0)
self.environ1.setScale(1)
self.environ2 = loader.loadModel("../data/models/skydome")
self.environ2.reparentTo(self._playing_node)
self.environ2.setP(180)
self.environ2.setH(270)
self.environ2.setScale(1)
self.dirnlight1 = DirectionalLight("dirn_light1")
self.dirnlight1.setColor(Vec4(1.0,1.0,1.0,1.0))
self.dirnlightnode1 = self._playing_node.attachNewNode(self.dirnlight1)
self.dirnlightnode1.setHpr(0,317,0)
self._playing_node.setLight(self.dirnlightnode1)
self.alight = AmbientLight('alight')
self.alight.setColor(VBase4(0.05, 0.05, 0.05, 1))
self.alight_node = self._playing_node.attachNewNode(self.alight)
self._playing_node.setLight(self.alight_node)
self.environ1 = loader.loadModel("../data/models/ground")
self.environ1.reparentTo(self._playing_node)
self.environ1.setPos(0,0,0)
self.environ1.setScale(1)
def loadGUI(self):
self.vidas_imgs = list()
w = 0.24
for i in range(self.VIDAS):
image_warning = OnscreenImage(image = '../data/Texture/signal_warning.png', pos=(-1 + i*w, 0, 0.85), parent=self._playing_node2d)
image_warning.setScale(0.1)
image_warning.setTransparency(TransparencyAttrib.MAlpha)
image_warning.hide()
image_ok = OnscreenImage(image = '../data/Texture/signal_ok.png', pos=(-1 + i*w, 0, 0.85), parent=self._playing_node2d)
image_ok.setScale(0.1)
image_ok.setTransparency(TransparencyAttrib.MAlpha)
image_ok.show()
self.vidas_imgs.append((image_ok, image_warning))
self._level_time_O = OnscreenText(text = '', pos = (0, 0.85), scale = 0.14, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0), parent=self._playing_node2d)
def loadMap(self):
if (self._modulos is not None):
for m in self._modulos:
m.remove()
if (self._paneles is not None):
for p in self._paneles:
p.remove()
self._tp = TiledParser("map"+str(self._num_lvl))
self._modulos, self._paneles = self._tp.load_models(self.world, self._playing_node)
def setAI(self):
taskMgr.add(self.update, 'Update')
def update(self, task):
if (task.frame > 1):
self.world.doPhysics(globalClock.getDt())
self._level_time -= globalClock.getDt()
self._level_time_O.setText(str(int(self._level_time)))
for panel in self._paneles:
contact = self.world.contactTestPair(self._player.getRBNode(), panel.getRBNode())
if contact.getNumContacts() > 0:
panel.manipulate()
brokens = 0
for panel in self._paneles:
if panel.isBroken():
brokens += 1
#print brokens
for i, vida_imgs in enumerate(self.vidas_imgs):
if i < len(self.vidas_imgs) - brokens:
vida_imgs[0].show()
vida_imgs[1].hide()
else:
vida_imgs[0].hide()
vida_imgs[1].show()
if brokens > self.VIDAS:
self.gameOver()
return task.done
if self._level_time <= 0:
self._num_lvl += 1
if self._num_lvl <= self._num_lvls:
self.nextLevel()
else:
self.theEnd()
return task.done
return task.cont
def gameOver(self):
taskMgr.remove('player-task')
taskMgr.remove('panel-task')
taskMgr.remove('panel-sound-task')
self._mission_text_O = OnscreenText(text = 'Game Over', pos = (0, 0), scale = 0.5, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0))
taskMgr.add(self.gameOverTransition, 'game-over-transition')
#self.loadLevel()
print "Game Over"
def gameOverTransition(self, task):
base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2)
if task.time > 3.0:
self._mission_text_O.hide()
props = WindowProperties()
props.setCursorHidden(False)
base.win.requestProperties(props)
self._state_context.changeState(gameStateMenu.GameStateMenu(self._state_context))
print "MENU"
return task.done
return task.cont
def nextLevel(self):
taskMgr.remove('player-task')
taskMgr.remove('panel-task')
taskMgr.remove('panel-sound-task')
self._mission_text_O = OnscreenText(text = 'Mission\nComplete', pos = (0, 0), scale = 0.5, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0))
taskMgr.add(self.nextLevelTransition, 'next-Level-transition')
print "Mission Complete"
def nextLevelTransition(self, task):
base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2)
if task.time > 3.0:
print "Next Level"
self._mission_text_O.hide()
self.loadLevel()
return task.done
return task.cont
def theEnd(self):
taskMgr.remove('player-task')
taskMgr.remove('panel-task')
taskMgr.remove('panel-sound-task')
self._mission_text_O = OnscreenText(text = '. The End .', pos = (0, 0), scale = 0.5, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0))
taskMgr.add(self.theEndTransition, 'theEnd-transition')
#self.loadLevel()
print "Mission Complete"
def theEndTransition(self, task):
base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2)
if task.time > 3.0:
self._mission_text_O.hide()
props = WindowProperties()
props.setCursorHidden(False)
base.win.requestProperties(props)
self._state_context.changeState(gameStateMenu.GameStateMenu(self._state_context))
print "The End"
return task.done
return task.cont
class FloatingPoses(object):
"""
like freetabletopplacement and tableplacements
manipulation.floatingposes corresponds to freegrip
floatingposes doesn't take into account
the position and orientation of the object
it is "free" in position and rotation. No obstacles were considered.
In contrast, each item in regrasp.floatingposes
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 "freegrip"
"s" is attached to the end of "floatingposes"
"""
def __init__(self, objpath, gdb, handpkg, base):
"""
initialization
:param objpath: path of the object
:param base: for the loadFreeAirGrip --> genHandPairs functions (for collision detection)
author: weiwei
date: 20161215, tsukuba
"""
self.handpkg = handpkg
self.handname = handpkg.getHandName()
self.hand0 = handpkg.newHandNM(hndcolor=[1,0,0,.1])
self.hand1 = handpkg.newHandNM(hndcolor=[0,0,1,.1])
self.objtrimesh = trimesh.load_mesh(objpath)
self.objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces)
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
# mat4list is nested list, floatingposemat4 is list (the flat version of mat4lsit), they are essentially the same
self.mat4list = []
self.floatingposemat4 = []
# gridsfloatingposemat4s is self.floatingposemat4 translated to different grids
self.gridsfloatingposemat4s = []
self.icos = None
self.floatinggripids = []
self.floatinggripmat4s = []
self.floatinggripcontacts = []
self.floatinggripnormals = []
self.floatinggripjawwidth = []
self.floatinggripidfreeair = []
self.bulletworld = BulletWorld()
self.handpairList = []
self.gdb = gdb
self.__loadFreeAirGrip(base)
# for IK
self.floatinggripikfeas_rgt = []
self.floatinggripikfeas_handx_rgt = []
self.floatinggripikfeas_lft = []
self.floatinggripikfeas_handx_lft = []
# for ik-feasible pairs
self.floatinggrippairsids = []
self.floatinggrippairshndmat4s = []
self.floatinggrippairscontacts = []
self.floatinggrippairsnormals = []
self.floatinggrippairsjawwidths = []
self.floatinggrippairsidfreeairs = []
self.__genPandaRotmat4()
def __loadFreeAirGrip(self, base):
"""
load self.freegripids, 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.handpkg.getHandName())
if freeairgripdata is None:
raise ValueError("Plan the freeairgrip first!")
self.freegripids = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
def __genPandaRotmat4(self, icolevel=1, angles=[0,45,90,135,180,225,270,315]):
"""
generate panda3d rotmat4 using icospheres and rotationaangle each origin-vertex vector of the icosphere
:param icolevel, the default value 1 = 42vertices
:param angles, 8 directions by default
:return: a list of [pandarotmat4, ...] size of the inner list is size of the angles
author: weiwei
date: 20170221, tsukuba
"""
self.mat4list = []
self.icos = trimesh.creation.icosphere(icolevel)
initmat4 = self.objnp.getMat()
for vert in self.icos.vertices:
self.mat4list.append([])
self.objnp.lookAt(vert[0], vert[1], vert[2])
ytozmat4 = Mat4.rotateMat(-90, self.objnp.getMat().getRow3(0))
newobjmat4 = self.objnp.getMat()*ytozmat4
for angle in angles:
tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2))
tmppandamat4 = newobjmat4*tmppandamat4
self.mat4list[-1].append(tmppandamat4)
self.objnp.setMat(initmat4)
self.floatingposemat4 = [e for l in self.mat4list for e in l]
def genHandPairs(self, base, loadser=False):
self.handpairList = []
if loadser is False:
# hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
# hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
pairidlist = list(itertools.combinations(range(len(self.freegripids)), 2))
print len(pairidlist)/5000+1
for i in range(100,len(pairidlist),len(pairidlist)/5000+1):
# for i0, i1 in pairidlist:
i0, i1 = pairidlist[i]
print i, len(pairidlist)
self.hand0.setMat(pandanpmat4 = self.freegriprotmats[i0])
self.hand0.setJawwidth(self.freegripjawwidth[i0])
self.hand1.setMat(pandanpmat4 = self.freegriprotmats[i1])
self.hand1.setJawwidth(self.freegripjawwidth[i1])
hndbullnodei0 = cd.genCollisionMeshMultiNp(self.hand0.handnp, base.render)
hndbullnodei1 = cd.genCollisionMeshMultiNp(self.hand1.handnp, base.render)
result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1)
if not result.getNumContacts():
self.handpairList.append([self.freegripids[i0], self.freegripids[i1]])
pickle.dump(self.handpairList, open("tmp.pickle", mode="wb"))
else:
self.handpairList = pickle.load(open("tmp.pickle", mode="rb"))
def genFPandGs(self, grids, icolevel=1, angles=[0,45,90,135,180,225,270,315]):
"""
genterate floating poses and their grasps, this function leverages genPandaRotmat4 and transformGrips
:param icolevel
:param angles see genPandaRotmat4
:return:
author: weiwei
date: 20170221
"""
self.gridsfloatingposemat4s = []
self.__genPandaRotmat4(icolevel, angles)
for gridposition in grids:
for posemat4 in self.floatingposemat4:
tmpposemat4 = Mat4(posemat4)
tmpposemat4.setRow(3, Vec3(gridposition[0], gridposition[1], gridposition[2]))
self.gridsfloatingposemat4s.append(tmpposemat4)
self.transformGrips()
def saveToDB(self):
"""
save the floatingposes and their grasps to the database
:return:
author: weiwei
date: 20170221
"""
sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) == 0:
# the gridsfloatingposes for the self.dbobjname is not saved
sql = "INSERT INTO floatingposes(rotmat, idobject) VALUES "
for i in range(len(self.gridsfloatingposemat4s)):
# print i, "/", len(self.gridsfloatingposemat4s)
sql += "('%s', (SELECT idobject FROM object WHERE name LIKE '%s')), " % \
(dc.mat4ToStr(self.gridsfloatingposemat4s[i]), self.dbobjname)
sql = sql[:-2] + ";"
self.gdb.execute(sql)
sql = "SELECT * FROM floatinggrips,floatingposes,object,freeairgrip,hand WHERE \
floatinggrips.idfloatingposes = floatingposes.idfloatingposes AND \
floatingposes.idobject = object.idobject AND \
object.name LIKE '%s' AND floatinggrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = hand.idhand AND hand.name LIKE '%s'" % (self.dbobjname, self.handpkg.getHandName())
result = self.gdb.execute(sql)
if len(result) == 0:
for i in range(len(self.gridsfloatingposemat4s)):
sql = "SELECT floatingposes.idfloatingposes FROM floatingposes,object WHERE \
floatingposes.idobject = object.idobject AND \
floatingposes.rotmat LIKE '%s' AND \
object.name LIKE '%s'" % (dc.mat4ToStr(self.gridsfloatingposemat4s[i]), self.dbobjname)
result = self.gdb.execute(sql)[0]
if len(result) != 0:
idfloatingposes = result[0]
sql = "SELECT * FROM floatinggrips WHERE idfloatingposes = %d" % idfloatingposes
result = self.gdb.execute(sql)
if len(result) == 0:
if len(self.floatinggripmat4s[i]) != 0:
sql = "INSERT INTO floatinggrips(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfloatingposes, idfreeairgrip) VALUES "
for j in range(len(self.floatinggripmat4s[i])):
# print "gripposes", i, "/", len(self.gridsfloatingposemat4s)
# print "grips", j, "/", len(self.floatinggripmat4s[i])
cct0 = self.floatinggripcontacts[i][j][0]
cct1 = self.floatinggripcontacts[i][j][1]
cctn0 = self.floatinggripnormals[i][j][0]
cctn1 = self.floatinggripnormals[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.floatinggripmat4s[i][j]), str(self.floatinggripjawwidth[i][j]), \
idfloatingposes, self.floatinggripidfreeair[i][j])
sql = sql[:-2] + ";"
self.gdb.execute(sql)
def loadFromDB(self):
"""
load the floatingposes and their grasps from the database
:return:
author: weiwei
date: 20170227
"""
self.gridsfloatingposemat4s = []
self.floatinggripids = []
self.floatinggripmat4s = []
self.floatinggripcontacts = []
self.floatinggripnormals = []
self.floatinggripjawwidth = []
self.floatinggripidfreeair = []
sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
self.gridsfloatingposemat4s.append(dc.strToMat4(resultrow[1]))
idfloatingposes = resultrow[0]
sql = "SELECT floatinggrips.idfloatinggrips, floatinggrips.contactpoint0, \
floatinggrips.contactpoint1, floatinggrips.contactnormal0, \
floatinggrips.contactnormal1, floatinggrips.rotmat, floatinggrips.jawwidth, \
floatinggrips.idfreeairgrip FROM floatinggrips,freeairgrip,hand WHERE \
floatinggrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = hand.idhand AND floatinggrips.idfloatingposes = %d AND \
hand.name = '%s'" % (idfloatingposes, self.handpkg.getHandName())
result = self.gdb.execute(sql)
if len(result) != 0:
floatinggripids = []
floatinggripmat4s = []
floatinggripcontacts = []
floatinggripnormals = []
floatinggripjawwidths = []
floatinggripidfreeairs = []
for resultrow in result:
cct0 = dc.strToV3(resultrow[1])
cct1 = dc.strToV3(resultrow[2])
cctn0 = dc.strToV3(resultrow[3])
cctn1 = dc.strToV3(resultrow[4])
floatinggripids.append(int(resultrow[0]))
floatinggripmat4s.append(dc.strToMat4(resultrow[5]))
floatinggripcontacts.append([cct0, cct1])
floatinggripnormals.append([cctn0, cctn1])
floatinggripjawwidths.append(float(resultrow[6]))
floatinggripidfreeairs.append(int(resultrow[7]))
self.floatinggripids.append(floatinggripids)
self.floatinggripmat4s.append(floatinggripmat4s)
self.floatinggripcontacts.append(floatinggripcontacts)
self.floatinggripnormals.append(floatinggripnormals)
self.floatinggripjawwidth.append(floatinggripjawwidths)
self.floatinggripidfreeair.append(floatinggripidfreeairs)
else:
print 'Plan floating grips first!'
assert(False)
else:
assert('No object found!')
def transformGrips(self):
"""
transform the freeair grips to all rotmat4 in self.gridsfloatingposemat4s
:return:
author: weiwei
date: 20170221, tsukuba
"""
self.floatinggripmat4s = []
self.floatinggripcontacts = []
self.floatinggripnormals = []
self.floatinggripjawwidth = []
self.floatinggripidfreeair = []
for icomat4 in self.gridsfloatingposemat4s:
floatinggrips = self.transformGripsOnePose(icomat4)
self.floatinggripmat4s.append(floatinggrips[0])
self.floatinggripcontacts.append(floatinggrips[1])
self.floatinggripnormals.append(floatinggrips[2])
self.floatinggripjawwidth.append(floatinggrips[3])
self.floatinggripidfreeair.append(floatinggrips[4])
def transformGripsOnePose(self, rotmat4):
"""
transform the freeair grips to one specific rotmat4
:param rotmat4:
:return: [floatinggripmat4s, floatinggripcontacts, floatinggripnormals, floatinggripjawwidths, floatinggripidfreeairs]
each element in the list is also a list
"""
floatinggripmat4s = []
floatinggripcontacts = []
floatinggripnormals = []
floatinggripjawwidths = []
floatinggripidfreeairs = []
for i, gripmat4 in enumerate(self.freegriprotmats):
floatinggripmat4 = gripmat4 * rotmat4
cct0 = rotmat4.xformPoint(self.freegripcontacts[i][0])
cct1 = rotmat4.xformPoint(self.freegripcontacts[i][1])
cctn0 = rotmat4.xformPoint(self.freegripnormals[i][0])
cctn1 = rotmat4.xformPoint(self.freegripnormals[i][1])
floatinggripjawwidth = self.freegripjawwidth[i]
floatinggripidfreeair = self.freegripids[i]
floatinggripmat4s.append(floatinggripmat4)
floatinggripcontacts.append([cct0, cct1])
floatinggripnormals.append([cctn0, cctn1])
floatinggripjawwidths.append(floatinggripjawwidth)
floatinggripidfreeairs.append(floatinggripidfreeair)
return [floatinggripmat4s, floatinggripcontacts, floatinggripnormals,
floatinggripjawwidths, floatinggripidfreeairs]
def showIcomat4s(self, nodepath):
"""
show the pandamat4s generated by genPandaRotmat4
:param nodepath, where np to repreantTo, usually base.render
:return:
author: weiwei
date: 20170221, tsukuba
"""
for i, icomat4list in enumerate(self.mat4list):
vert = self.icos.vertices*100
spos = Vec3(vert[i][0], vert[i][1], vert[i][2])
for icomat4 in icomat4list:
pg.plotAxisSelf(nodepath, spos, icomat4, length=100, thickness=3)
def updateDBwithFGPairs(self, loadser = False):
"""
compute the floatinggrippairs using freegrippairs and
save the floatinggripspairs into Database
:param loadser whether use serialized data for handpairlist
:return:
author: weiwei
date: 20170301
"""
if len(self.handpairList) == 0:
self.genHandPairs(base, loadser)
tic = time.clock()
for fpid in range(len(self.gridsfloatingposemat4s)):
toc = time.clock()
print toc-tic
if fpid != 0:
print "remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic)
print fpid, len(self.gridsfloatingposemat4s)
# gen correspondence between freeairgripid and index
# indfgoffa means index of floatinggrips whose freeairgripid are xxx
indfgoffa = {}
for i in range(len(self.floatinggripidfreeair[fpid])):
indfgoffa[self.floatinggripidfreeair[fpid][i]] = i
# handidpair_indfg is the pairs using index of floatinggrips
handidpair_indfg = []
for handidpair in self.handpairList:
handidpair_indfg.append([indfgoffa[handidpair[0]], indfgoffa[handidpair[1]]])
# if handidpair_indfg[0] is right, 1 is left
sql = "INSERT IGNORE INTO floatinggripspairs VALUES (%d, %d)" % \
(self.floatinggripids[fpid][handidpair_indfg[-1][0]], self.floatinggripids[fpid][handidpair_indfg[-1][1]])
self.gdb.execute(sql)
# if 1 is right, 0 is left
sql = "INSERT IGNORE INTO floatinggripspairs VALUES (%d, %d)" % \
(self.floatinggripids[fpid][handidpair_indfg[-1][1]], self.floatinggripids[fpid][handidpair_indfg[-1][0]])
self.gdb.execute(sql)
def loadIKFeasibleFGPairsFromDB(self, robot):
"""
load the IK FeasibleFGPairs
:return:
author: weiwei
date: 20170301
"""
self.loadFromDB()
self.loadIKFromDB(robot)
idrobot = self.gdb.loadIdRobot(robot)
idarmrgt = self.gdb.loadIdArm('rgt')
idarmlft = self.gdb.loadIdArm('lft')
self.floatinggrippairsids = []
self.floatinggrippairshndmat4s = []
self.floatinggrippairscontacts = []
self.floatinggrippairsnormals = []
self.floatinggrippairsjawwidths = []
self.floatinggrippairsidfreeairs = []
for fpid in range(len(self.gridsfloatingposemat4s)):
sql = "SELECT floatingposes.idfloatingposes FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s' AND floatingposes.rotmat LIKE '%s'" % \
(self.dbobjname, dc.mat4ToStr(self.gridsfloatingposemat4s[fpid]))
result = self.gdb.execute(sql)
if len(result) != 0:
idfloatingposes = result[0][0]
floatinggrippairsids = []
floatinggrippairshndmat4s = []
floatinggrippairscontacts = []
floatinggrippairsnormals = []
floatinggrippairsjawwidths = []
floatinggrippairsidfreeairs = []
sql = "SELECT floatinggripspairs.idfloatinggrips0, floatinggripspairs.idfloatinggrips1, \
fg0.contactpoint0, fg0.contactpoint1, fg0.contactnormal0, fg0.contactnormal1, fg0.rotmat, \
fg0.jawwidth, fg0.idfreeairgrip, \
fg1.contactpoint0, fg1.contactpoint1, fg1.contactnormal0, fg1.contactnormal1, fg1.rotmat, \
fg1.jawwidth, fg1.idfreeairgrip FROM floatinggripspairs, floatinggrips fg0, floatinggrips fg1, \
ikfloatinggrips ikfg0, ikfloatinggrips ikfg1 WHERE \
floatinggripspairs.idfloatinggrips0 = fg0.idfloatinggrips AND \
floatinggripspairs.idfloatinggrips1 = fg1.idfloatinggrips AND \
fg0.idfloatingposes = %d AND fg1.idfloatingposes = %d AND \
fg0.idfloatinggrips = ikfg0.idfloatinggrips AND ikfg0.feasibility like 'True' AND ikfg0.feasibility_handx like 'True' AND \
ikfg0.idrobot = %d AND ikfg0.idarm = %d AND \
fg1.idfloatinggrips = ikfg1.idfloatinggrips AND ikfg1.feasibility like 'True' AND ikfg1.feasibility_handx like 'True' AND \
ikfg1.idrobot = %d AND ikfg1.idarm = %d" % (idfloatingposes, idfloatingposes, idrobot, idarmrgt, idrobot, idarmlft)
result = self.gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
floatinggrippairsids.append([resultrow[0], resultrow[1]])
floatinggrippairshndmat4s.append([dc.strToMat4(resultrow[6]), dc.strToMat4(resultrow[13])])
rgtcct0 = dc.strToV3(resultrow[2])
rgtcct1 = dc.strToV3(resultrow[3])
lftcct0 = dc.strToV3(resultrow[9])
lftcct1 = dc.strToV3(resultrow[10])
floatinggrippairscontacts.append([[rgtcct0, rgtcct1], [lftcct0, lftcct1]])
rgtcctn0 = dc.strToV3(resultrow[4])
rgtcctn1 = dc.strToV3(resultrow[5])
lftcctn0 = dc.strToV3(resultrow[11])
lftcctn1 = dc.strToV3(resultrow[12])
floatinggrippairsnormals.append([[rgtcctn0, rgtcctn1], [lftcctn0, lftcctn1]])
floatinggrippairsjawwidths.append([float(resultrow[7]), float(resultrow[14])])
floatinggrippairsidfreeairs.append([int(resultrow[8]), int(resultrow[15])])
self.floatinggrippairsids.append(floatinggrippairsids)
self.floatinggrippairshndmat4s.append(floatinggrippairshndmat4s)
self.floatinggrippairscontacts.append(floatinggrippairscontacts)
self.floatinggrippairsnormals.append(floatinggrippairsnormals)
self.floatinggrippairsjawwidths.append(floatinggrippairsjawwidths)
self.floatinggrippairsidfreeairs.append(floatinggrippairsidfreeairs)
# for i,pairs in enumerate(self.floatinggrippairsids):
# print i
# print pairs
def updateDBwithIK(self, robot):
"""
compute the IK feasible grasps of each pose
:return:
"""
# load retraction distances
rethandx, retworldz, retworlda, worldz = self.gdb.loadIKRet()
# load idrobot
idrobot = self.gdb.loadIdRobot(robot)
self.floatinggripikfeas_rgt = []
self.floatinggripikfeas_handx_rgt = []
self.floatinggripikfeas_lft = []
self.floatinggripikfeas_handx_lft = []
tic = time.clock()
for fpid in range(len(self.gridsfloatingposemat4s)):
toc = time.clock()
print toc-tic
if fpid != 0:
print "remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic)
print fpid, len(self.gridsfloatingposemat4s)
### right hand
armname = 'rgt'
feasibility = []
feasibility_handx = []
for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]):
feasibility.append('False')
feasibility_handx.append('False')
fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0]+self.floatinggripcontacts[fpid][i][1])/2
fpgsfgrcenternp = pg.v3ToNp(fpgsfgrcenter)
fpgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
minusworldy = Vec3(0,-1,0)
if Vec3(handx).angleDeg(minusworldy) < 60:
fpgsfgrcenternp_handx = pg.v3ToNp(fpgsfgrcenter+handx*rethandx)
if robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armname) is not None:
feasibility[i] = 'True'
if robot.numik(fpgsfgrcenternp_handx, fpgsrotmat3np, armname) is not None:
feasibility_handx[i] = 'True'
self.floatinggripikfeas_rgt.append(feasibility)
self.floatinggripikfeas_handx_rgt.append(feasibility_handx)
### left hand
armname = 'lft'
feasibility = []
feasibility_handx = []
for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]):
feasibility.append('False')
feasibility_handx.append('False')
fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0]+self.floatinggripcontacts[fpid][i][1])/2
fpgsfgrcenternp = pg.v3ToNp(fpgsfgrcenter)
fpgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
plusworldy = Vec3(0,1,0)
if Vec3(handx).angleDeg(plusworldy) < 60:
fpgsfgrcenternp_handx = pg.v3ToNp(fpgsfgrcenter+handx*rethandx)
if robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armname) is not None:
feasibility[i] = 'True'
if robot.numik(fpgsfgrcenternp_handx, fpgsrotmat3np, armname) is not None:
feasibility_handx[i] = 'True'
self.floatinggripikfeas_lft.append(feasibility)
self.floatinggripikfeas_handx_lft.append(feasibility_handx)
self.saveIKtoDB(idrobot)
def saveIKtoDB(self, idrobot):
"""
saveupdated IK to DB
this function is separated from updateDBwithIK for illustration
:return:
"""
for fpid in range(len(self.gridsfloatingposemat4s)):
# right arm
idarm = self.gdb.loadIdArm('rgt')
for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
sql = "INSERT IGNORE INTO ikfloatinggrips(idrobot, idarm, idfloatinggrips, feasibility, \
feasibility_handx) VALUES (%d, %d, %d, '%s', '%s')" \
% (idrobot, idarm, idfloatinggrips, self.floatinggripikfeas_rgt[fpid][i],
self.floatinggripikfeas_handx_rgt[fpid][i])
gdb.execute(sql)
# left arm
idarm = self.gdb.loadIdArm('lft')
for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
sql = "INSERT IGNORE INTO ikfloatinggrips(idrobot, idarm, idfloatinggrips, feasibility, \
feasibility_handx) VALUES (%d, %d, %d, '%s', '%s')" \
% (idrobot, idarm, idfloatinggrips, self.floatinggripikfeas_lft[fpid][i],
self.floatinggripikfeas_handx_lft[fpid][i])
gdb.execute(sql)
def loadIKFromDB(self, robot):
"""
load the feasibility of IK from db
:param robot:
:return:
author: weiwei
date: 20170301
"""
idrobot = self.gdb.loadIdRobot(robot)
self.floatinggripikfeas_rgt = []
self.floatinggripikfeas_handx_rgt = []
self.floatinggripikfeas_lft = []
self.floatinggripikfeas_handx_lft = []
for fpid in range(len(self.gridsfloatingposemat4s)):
# right arm
feasibility = []
feasibility_handx = []
idarm = self.gdb.loadIdArm('rgt')
for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
feasibility.append('False')
feasibility_handx.append('False')
sql = "SELECT feasibility, feasibility_handx FROM ikfloatinggrips WHERE \
idrobot = %d AND idarm = %d and idfloatinggrips = %d" % (idrobot, idarm, idfloatinggrips)
result = self.gdb.execute(sql)
if len(result) != 0:
feasibility[i] = result[0][0]
feasibility_handx[i] = result[0][1]
self.floatinggripikfeas_rgt.append(feasibility)
self.floatinggripikfeas_handx_rgt.append(feasibility_handx)
# left arm
feasibility = []
feasibility_handx = []
idarm = self.gdb.loadIdArm('lft')
for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
feasibility.append('False')
feasibility_handx.append('False')
sql = "SELECT feasibility, feasibility_handx FROM ikfloatinggrips WHERE \
idrobot = %d AND idarm = %d and idfloatinggrips = %d" % (idrobot, idarm, idfloatinggrips)
result = self.gdb.execute(sql)
if len(result) != 0:
feasibility[i] = result[0][0]
feasibility_handx[i] = result[0][1]
self.floatinggripikfeas_lft.append(feasibility)
self.floatinggripikfeas_handx_lft.append(feasibility_handx)
def plotOneFPandG(self, parentnp, fpid=0):
"""
plot the objpose and grasps at a specific rotmat4
:param fpid: the index of self.floatingposemat4
:return:
author: weiwei
date: 20170221, tsukuba
"""
objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces)
objnp.setMat(self.gridsfloatingposemat4s[fpid])
objnp.reparentTo(parentnp)
for i, hndrotmat4 in enumerate(self.gridsfloatingposemat4s[fpid]):
if i == 7:
# show grasps
hand0 = self.handpkg.newHandNM(hndcolor=[0, 1, 0, 1])
hand0.setMat(pandanpmat4 = hndrotmat4)
hand0.setJawwidth(self.floatinggripjawwidth[fpid][i])
hand0.reparentTo(parentnp)
print self.handpairList
for handidpair in self.handpairList:
if handidpair[0] == self.floatinggripidfreeair[fpid][i]:
pairedilist = [i1 for i1 in range(len(self.floatinggripidfreeair[fpid]))
if self.floatinggripidfreeair[fpid][i1]==handidpair[1]]
print pairedilist
i1 = pairedilist[0]
# if self.floatinggripikfeas_lft[fpid][i1] == 'True':
hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1])
hndrotmat4 = self.floatinggripmat4s[fpid][i1]
hand1.setMat(pandanpmat4 = hndrotmat4)
hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1])
hand1.reparentTo(parentnp)
if handidpair[1] == self.floatinggripidfreeair[fpid][i]:
pairedilist = [i1 for i1 in range(len(self.floatinggripidfreeair[fpid]))
if self.floatinggripidfreeair[fpid][i1]==handidpair[0]]
print pairedilist
i1 = pairedilist[0]
# if self.floatinggripikfeas_lft[fpid][i1] == 'True':
hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1])
hndrotmat4 = self.floatinggripmat4s[fpid][i1]
hand1.setMat(pandanpmat4 = hndrotmat4)
hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1])
hand1.reparentTo(parentnp)
def plotOneFPandGPairs(self, parentnp, fpid=0):
"""
plot the gpairss at a specific rotmat4
:param fpid: the index of self.floatingposemat4
:return:
author: weiwei
date: 20170301, tsukuba
"""
objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces)
objnp.setMat(self.gridsfloatingposemat4s[fpid])
objnp.setColor(Vec4(.7,0.3,0,1))
objnp.reparentTo(parentnp)
print self.floatinggrippairshndmat4s[fpid]
for i, hndrotmat4pair in enumerate(self.floatinggrippairshndmat4s[fpid]):
# if i == 9:
# show grasps
hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .5])
hand0mat4 = Mat4(hndrotmat4pair[0])
# h0row3 = hand0mat4.getRow3(3)
# h0row3[2] = h0row3[2]+i*20.0
# hand0mat4.setRow(3, h0row3[2])
hand0.setMat(pandanpmat4 = hand0mat4)
hand0.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][0])
hand0.reparentTo(parentnp)
hand1 = self.handpkg.newHandNM(hndcolor=[0, .0, 1, .5])
hand1mat4 = Mat4(hndrotmat4pair[1])
# h1row3 = hand1mat4.getRow3(3)
# h1row3[2] = h1row3[2]+i*20.0
# hand1mat4.setRow(3, h1row3[2])
hand1.setMat(pandanpmat4 = hand1mat4)
hand1.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][1])
hand1.reparentTo(parentnp)
class TwoObjAss(object):
"""
the class uses genAvailableFAGs to compute grasps for the assembly of two objects
different rotations could be saved to database
"""
def __init__(self, base, obj0path, obj0Mat4, obj1path, obj1Mat4, assDirect1to0, gdb, handpkg):
"""
initiliazation
obj0 will be manipulated by rgt hand, obj1 will be manipulated by lft hand
# see genAvailableFAGs function for the details of parameters
:param base:
:param obj0path:
:param obj0Mat4:
:param obj1path:
:param obj1Mat4:
:param assDirect1to0: the assembly direction to assemble object 1 to object 0 (with length)
:param gdb:
:param handpkg:
"""
self.base = base
self.obj0path = obj0path
self.obj0Mat4 = obj0Mat4
self.dbobj0name = os.path.splitext(os.path.basename(obj0path))[0]
self.obj1path = obj1path
self.obj1Mat4 = obj1Mat4
self.dbobj1name = os.path.splitext(os.path.basename(obj1path))[0]
self.assDirect1to0 = assDirect1to0
self.gdb = gdb
self.handpkg = handpkg
# define the free ass0grip and ass1grip
# definition: objgrips: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair]
self.obj0grips = []
self.obj1grips = []
# ico means they corresponds to ico rotmats
self.gridsfloatingposemat4s = []
# right grips for each gridsfloatingposemat4
self.icoass0gripids = []
self.icoass0gripcontacts = []
self.icoass0gripnormals = []
self.icoass0griprotmat4s = []
self.icoass0gripjawwidth = []
self.icoass0gripidfreeair = []
# left grips for each gridsfloatingposemat4
self.icoass1gripids = []
self.icoass1gripcontacts = []
self.icoass1gripnormals = []
self.icoass1griprotmat4s = []
self.icoass1gripjawwidth = []
self.icoass1gripidfreeair = []
# ik related
self.icoass0gripsik = []
self.icoass0gripsik_retassdir = []
self.icoass1gripsik = []
self.icoass1gripsik_retassdir = []
# handpairList
self.handpairList = []
self.bulletworld = BulletWorld()
# ik feasible pairs for each gridsfloatingposemat4
self.icoassgrippairsids = []
self.icoassgrippairscontacts = []
self.icoassgrippairsnormals = []
self.icoassgrippairshndmat4s = []
self.icoassgrippairsjawwidths = []
self.icoassgrippairsidfreeairs = []
def genXAssPandGs(self, grids, icolevel=1, angles=[0,45,90,135,180,225,270,315]):
"""
genterate xass poses and their grasps, this function leverages genPandaRotmat4 and transformGrips
:param icolevel
:param angles see genPandaRotmat4
:return:
author: weiwei
date: 20170221
"""
self.gridsfloatingposemat4s = []
self.__genPandaRotmat4(icolevel, angles)
for gridposition in grids:
for posemat4 in self.floatingposemat4:
tmpposemat4 = Mat4(posemat4)
tmpposemat4.setRow(3, Vec3(gridposition[0], gridposition[1], gridposition[2]))
self.gridsfloatingposemat4s.append(tmpposemat4)
if len(self.obj0grips) == 0 or len(self.obj1grips) == 0:
self.__genFreeAssGrips()
self.__transformGrips()
def saveToDB(self):
"""
:return:
"""
self.icoass0gripids = []
self.icoass1gripids = []
# save to assembly table, 0-right, 1-left
idassembly = self.gdb.loadIdAssembly(self.dbobj0name, self.obj0Mat4, self.dbobj1name, self.obj1Mat4, self.assDirect1to0)
# check if rotation already exist
sql = "SELECT * FROM assemblyx WHERE assemblyx.idassembly = %d" % idassembly
result = self.gdb.execute(sql)
if len(result) == 0:
# save rotation and grasps
# if assemblyx do not exist, assemblyxgrip0/1 dont exist
for i, poserotmat4 in enumerate(self.gridsfloatingposemat4s):
sql = "INSERT INTO assemblyx(idassembly, rotmat) VALUES (%d, '%s')" % (idassembly, dc.mat4ToStr(poserotmat4))
idassemblyx = self.gdb.execute(sql)
# object 0
self.icoass0gripids.append([])
for j, hndrotmat4 in enumerate(self.icoass0griprotmat4s[i]):
strcct0 = dc.v3ToStr(self.icoass0gripcontacts[i][j][0])
strcct1 = dc.v3ToStr(self.icoass0gripcontacts[i][j][1])
strcctn0 = dc.v3ToStr(self.icoass0gripnormals[i][j][0])
strcctn1 = dc.v3ToStr(self.icoass0gripnormals[i][j][1])
strrotmat = dc.mat4ToStr(hndrotmat4)
strjawwidth = str(self.icoass0gripjawwidth[i][j])
idfreeairgrip = self.icoass0gripidfreeair[i][j]
sql = "INSERT INTO assemblyxgrips0(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idassemblyx, idfreeairgrip) VALUES ('%s', '%s', '%s', '%s', '%s', '%s', %d, \
%d)" % (strcct0, strcct1, strcctn0, strcctn1, strrotmat, strjawwidth, idassemblyx, idfreeairgrip)
idaxg = self.gdb.execute(sql)
self.icoass0gripids[-1].append(idaxg)
# object 1
self.icoass1gripids.append([])
for j, hndrotmat4 in enumerate(self.icoass1griprotmat4s[i]):
strcct0 = dc.v3ToStr(self.icoass1gripcontacts[i][j][0])
strcct1 = dc.v3ToStr(self.icoass1gripcontacts[i][j][1])
strcctn0 = dc.v3ToStr(self.icoass1gripnormals[i][j][0])
strcctn1 = dc.v3ToStr(self.icoass1gripnormals[i][j][1])
strrotmat = dc.mat4ToStr(hndrotmat4)
strjawwidth = str(self.icoass1gripjawwidth[i][j])
idfreeairgrip = self.icoass1gripidfreeair[i][j]
sql = "INSERT INTO assemblyxgrips1(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idassemblyx, idfreeairgrip) VALUES ('%s', '%s', '%s', '%s', '%s', '%s', %d, \
%d)" % (strcct0, strcct1, strcctn0, strcctn1, strrotmat, strjawwidth, idassemblyx, idfreeairgrip)
idaxg = self.gdb.execute(sql)
self.icoass1gripids[-1].append(idaxg)
# TODO: Have to delete assemblyx when grasps are deleted
# TODO: write algorithms to enable insertion of grasps using idassemblyx
def loadFromDB(self):
"""
:return:
"""
self.gridsfloatingposemat4s = []
self.icoass0gripids = []
self.icoass0gripcontacts = []
self.icoass0gripnormals = []
self.icoass0griprotmat4s = []
self.icoass0gripjawwidth = []
self.icoass0gripidfreeair = []
self.icoass1gripids = []
self.icoass1gripcontacts = []
self.icoass1gripnormals = []
self.icoass1griprotmat4s = []
self.icoass1gripjawwidth = []
self.icoass1gripidfreeair = []
idassembly = self.gdb.loadIdAssembly(self.dbobj0name, self.obj0Mat4, self.dbobj1name, self.obj1Mat4, self.assDirect1to0)
# check if rotation already exist
sql = "SELECT assemblyx.idassemblyx, assemblyx.rotmat FROM assemblyx WHERE assemblyx.idassembly = %d" % idassembly
result = self.gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
poserotmat4 = dc.strToMat4(resultrow[1])
self.gridsfloatingposemat4s.append(poserotmat4)
idassemblyx = resultrow[0]
# g0
sql = "SELECT * FROM assemblyxgrips0 WHERE idassemblyx = %d" % idassemblyx
result = self.gdb.execute(sql)
if len(result) != 0:
self.icoass0gripids.append([])
self.icoass0gripcontacts.append([])
self.icoass0gripnormals.append([])
self.icoass0griprotmat4s.append([])
self.icoass0gripjawwidth.append([])
self.icoass0gripidfreeair.append([])
for resultrow in result:
self.icoass0gripids[-1].append(resultrow[0])
cct0 = dc.strToV3(resultrow[1])
cct1 = dc.strToV3(resultrow[2])
cctn0 = dc.strToV3(resultrow[3])
cctn1 = dc.strToV3(resultrow[4])
self.icoass0gripcontacts[-1].append([cct0, cct1])
self.icoass0gripnormals[-1].append([cctn0, cctn1])
self.icoass0griprotmat4s[-1].append(dc.strToMat4(resultrow[5]))
self.icoass0gripjawwidth[-1].append(float(resultrow[6]))
self.icoass0gripidfreeair[-1].append(int(resultrow[8]))
# g1
sql = "SELECT * FROM assemblyxgrips1 WHERE idassemblyx = %d" % idassemblyx
result = self.gdb.execute(sql)
if len(result) != 0:
self.icoass1gripids.append([])
self.icoass1gripcontacts.append([])
self.icoass1gripnormals.append([])
self.icoass1griprotmat4s.append([])
self.icoass1gripjawwidth.append([])
self.icoass1gripidfreeair.append([])
for resultrow in result:
self.icoass1gripids[-1].append(resultrow[0])
cct0 = dc.strToV3(resultrow[1])
cct1 = dc.strToV3(resultrow[2])
cctn0 = dc.strToV3(resultrow[3])
cctn1 = dc.strToV3(resultrow[4])
self.icoass1gripcontacts[-1].append([cct0, cct1])
self.icoass1gripnormals[-1].append([cctn0, cctn1])
self.icoass1griprotmat4s[-1].append(dc.strToMat4(resultrow[5]))
self.icoass1gripjawwidth[-1].append(float(resultrow[6]))
self.icoass1gripidfreeair[-1].append(int(resultrow[8]))
def updateDBwithHandPairs(self):
"""
compute the assgrippairs using assfreegrippairs and
save the assgrippairs into Database
:return:
author: weiwei
date: 20170307
"""
self.__genHandPairs(base)
tic = time.clock()
for fpid in range(len(self.gridsfloatingposemat4s)):
toc = time.clock()
print toc-tic
if fpid != 0:
print "remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic)
print fpid, len(self.gridsfloatingposemat4s)
# gen correspondence between freeairgripid and index
# indagoffa means index of assgrips whose freeairgripid are xxx
indagoffa0 = {}
for i in range(len(self.icoass0gripidfreeair[fpid])):
indagoffa0[self.icoass0gripidfreeair[fpid][i]] = i
indagoffa1 = {}
for i in range(len(self.icoass1gripidfreeair[fpid])):
indagoffa1[self.icoass1gripidfreeair[fpid][i]] = i
# handidpair_indag is the pairs using index of floatinggrips
handidpair_indag = []
for handidpair in self.handpairList:
handidpair_indag.append([indagoffa0[handidpair[0]], indagoffa1[handidpair[1]]])
sql = "INSERT IGNORE INTO assemblyxgrippairs VALUES (%d, %d)" % \
(self.icoass0gripids[fpid][handidpair_indag[-1][0]], self.icoass1gripids[fpid][handidpair_indag[-1][1]])
self.gdb.execute(sql)
def updateDBwithIK(self, robot):
"""
compute the IK feasible grasps of each pose
:return:
"""
# load idrobot
idrobot = self.gdb.loadIdRobot(robot)
self.assgikfeas0 = []
self.assgikfeas0_retassdir = []
self.assgikfeas1= []
self.assgikfeas1_retassdir = []
tic = time.clock()
for fpid, apmat in enumerate(self.gridsfloatingposemat4s):
assdir1to0 = apmat.xformVec(self.assDirect1to0)
assdir0to1 = apmat.xformVec(-self.assDirect1to0)
toc = time.clock()
print toc-tic
if fpid != 0:
print "remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic)
print fpid, len(self.gridsfloatingposemat4s)
### right hand # rgt = 0
armname = 'rgt'
assgikfeas0 = []
assgikfeas0_retassdir = []
for i, hndrotmat4 in enumerate(self.icoass0griprotmat4s[fpid]):
assgikfeas0.append('False')
assgikfeas0_retassdir.append('False')
assgsfgrcenter = (self.icoass0gripcontacts[fpid][i][0]+self.icoass0gripcontacts[fpid][i][1])/2
assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter)
assgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
minusworldy = Vec3(0,-1,0)
if Vec3(handx).angleDeg(minusworldy) < 60:
assgsfgrcenternp_retassdir = pg.v3ToNp(assgsfgrcenter+assdir1to0)
if robot.numik(assgsfgrcenternp, assgsrotmat3np, armname) is not None:
assgikfeas0[i] = 'True'
if robot.numik(assgsfgrcenternp_retassdir, assgsrotmat3np, armname) is not None:
assgikfeas0_retassdir[i] = 'True'
self.icoass0gripsik.append(assgikfeas0)
self.icoass0gripsik_retassdir.append(assgikfeas0_retassdir)
### left hand
armname = 'lft'
assgikfeas1 = []
assgikfeas1_retassdir = []
for i, hndrotmat4 in enumerate(self.icoass1griprotmat4s[fpid]):
assgikfeas1.append('False')
assgikfeas1_retassdir.append('False')
assgsfgrcenter = (self.icoass1gripcontacts[fpid][i][0]+self.icoass1gripcontacts[fpid][i][1])/2
assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter)
assgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
plusworldy = Vec3(0,1,0)
if Vec3(handx).angleDeg(plusworldy) < 60:
assgsfgrcenternp_retassdir = pg.v3ToNp(assgsfgrcenter+assdir0to1)
if robot.numik(assgsfgrcenternp, assgsrotmat3np, armname) is not None:
assgikfeas1[i] = 'True'
if robot.numik(assgsfgrcenternp_retassdir, assgsrotmat3np, armname) is not None:
assgikfeas1_retassdir[i] = 'True'
self.icoass1gripsik.append(assgikfeas1)
self.icoass1gripsik_retassdir.append(assgikfeas1_retassdir)
self.__saveIKtoDB(idrobot)
def loadIKFeasibleAGPairsFromDB(self, robot):
"""
load the IK FeasibleAGPairs AG -> assgrippairs
:return:
author: weiwei
date: 20170301
"""
self.loadFromDB()
self.__loadIKFromDB(robot)
idrobot = self.gdb.loadIdRobot(robot)
idarmrgt = self.gdb.loadIdArm('rgt')
idarmlft = self.gdb.loadIdArm('lft')
self.icoassgrippairsids = []
self.icoassgrippairscontacts = []
self.icoassgrippairsnormals = []
self.icoassgrippairshndmat4s = []
self.icoassgrippairsjawwidths = []
self.icoassgrippairsidfreeairs = []
idassembly = self.gdb.loadIdAssembly(self.dbobj0name, self.obj0Mat4, self.dbobj1name, self.obj1Mat4, self.assDirect1to0)
for fpid in range(len(self.gridsfloatingposemat4s)):
sql = "SELECT assemblyx.idassemblyx FROM assemblyx WHERE assemblyx.idassembly = %d AND \
assemblyx.rotmat LIKE '%s'" % (idassembly, dc.mat4ToStr(self.gridsfloatingposemat4s[fpid]))
result = self.gdb.execute(sql)
if len(result) != 0:
idfloatingposes = result[0][0]
icoassgrippairsids = []
icoassgrippairscontacts = []
icoassgrippairsnormals = []
icoassgrippairshndmat4s = []
icoassgrippairsjawwidths = []
icoassgrippairsidfreeairs = []
sql = "SELECT assemblyxgrippairs.idassemblyxgrips0, assemblyxgrippairs.idassemblyxgrips1, \
assemblyxgrips0.contactpoint0, assemblyxgrips0.contactpoint1, assemblyxgrips0.contactnormal0, \
assemblyxgrips0.contactnormal1, assemblyxgrips0.rotmat, \
assemblyxgrips0.jawwidth, assemblyxgrips0.idfreeairgrip, \
assemblyxgrips1.contactpoint0, assemblyxgrips1.contactpoint1, assemblyxgrips1.contactnormal0, \
assemblyxgrips1.contactnormal1, assemblyxgrips1.rotmat, \
assemblyxgrips1.jawwidth, assemblyxgrips1.idfreeairgrip \
FROM assemblyxgrippairs, assemblyxgrips0, assemblyxgrips1, \
ikassemblyxgrips0, ikassemblyxgrips1 WHERE \
assemblyxgrippairs.idassemblyxgrips0 = assemblyxgrips0.idassemblyxgrips0 AND \
assemblyxgrippairs.idassemblyxgrips1 = assemblyxgrips1.idassemblyxgrips1 AND \
assemblyxgrips0.idassemblyx = %d AND assemblyxgrips1.idassemblyx = %d AND \
assemblyxgrips0.idassemblyxgrips0 = ikassemblyxgrips0.idassemblyxgrips0 AND \
ikassemblyxgrips0.feasibility like 'True' AND ikassemblyxgrips0.feasibility_assdir like 'True' AND \
ikassemblyxgrips0.idrobot = %d AND ikassemblyxgrips0.idarm = %d AND \
assemblyxgrips1.idassemblyxgrips1 = ikassemblyxgrips1.idassemblyxgrips1 AND \
ikassemblyxgrips1.feasibility like 'True' AND ikassemblyxgrips1.feasibility_assdir like 'True' AND \
ikassemblyxgrips1.idrobot = %d AND ikassemblyxgrips1.idarm = %d" % \
(idfloatingposes, idfloatingposes, idrobot, idarmrgt, idrobot, idarmlft)
result = self.gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
icoassgrippairsids.append([resultrow[0], resultrow[1]])
rgtcct0 = dc.strToV3(resultrow[2])
rgtcct1 = dc.strToV3(resultrow[3])
lftcct0 = dc.strToV3(resultrow[9])
lftcct1 = dc.strToV3(resultrow[10])
icoassgrippairscontacts.append([[rgtcct0, rgtcct1], [lftcct0, lftcct1]])
rgtcctn0 = dc.strToV3(resultrow[4])
rgtcctn1 = dc.strToV3(resultrow[5])
lftcctn0 = dc.strToV3(resultrow[11])
lftcctn1 = dc.strToV3(resultrow[12])
icoassgrippairsnormals.append([[rgtcctn0, rgtcctn1], [lftcctn0, lftcctn1]])
icoassgrippairshndmat4s.append([dc.strToMat4(resultrow[6]), dc.strToMat4(resultrow[13])])
icoassgrippairsjawwidths.append([float(resultrow[7]), float(resultrow[14])])
icoassgrippairsidfreeairs.append([int(resultrow[8]), int(resultrow[15])])
self.icoassgrippairsids.append(icoassgrippairsids)
self.icoassgrippairscontacts.append(icoassgrippairscontacts)
self.icoassgrippairsnormals.append(icoassgrippairsnormals)
self.icoassgrippairshndmat4s.append(icoassgrippairshndmat4s)
self.icoassgrippairsjawwidths.append(icoassgrippairsjawwidths)
self.icoassgrippairsidfreeairs.append(icoassgrippairsidfreeairs)
def __genHandPairs(self, base):
self.handpairList = []
self.__genFreeAssGrips()
ass0gripcontacts, ass0gripnormals, ass0griprotmat4s, ass0gripjawwidth, ass0gripidfreeair = self.obj0grips
ass1gripcontacts, ass1gripnormals, ass1griprotmat4s, ass1gripjawwidth, ass1gripidfreeair = self.obj1grips
hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
i0list = range(len(ass0gripidfreeair))
i1list = range(len(ass1gripidfreeair))
pairidlist = list(itertools.product(i0list, i1list))
print len(pairidlist)/10000+1
for i in range(0,len(pairidlist),len(pairidlist)/10000+1):
# for i0, i1 in pairidlist:
i0, i1 = pairidlist[i]
print i, len(pairidlist)
hand0.setMat(ass0griprotmat4s[i0])
hand0.setJawwidth(ass0gripjawwidth[i0])
hand1.setMat(ass1griprotmat4s[i1])
hand1.setJawwidth(ass1gripjawwidth[i1])
hndbullnodei0 = cd.genCollisionMeshMultiNp(hand0.handnp, base.render)
hndbullnodei1 = cd.genCollisionMeshMultiNp(hand1.handnp, base.render)
result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1)
if not result.getNumContacts():
self.handpairList.append([ass0gripidfreeair[i0], ass1gripidfreeair[i1]])
# print ass0gripidfreeair
# print self.icoass0gripidfreeair[0]
# print ass1gripidfreeair
# print self.icoass1gripidfreeair[0]
# print self.handpairList
# assert "compare"
def __saveIKtoDB(self, idrobot):
"""
saveupdated IK to DB
this function is separated from updateDBwithIK for illustration
:return:
"""
for fpid in range(len(self.gridsfloatingposemat4s)):
# right arm
idarm = self.gdb.loadIdArm('rgt')
for i, idicoass0grip in enumerate(self.icoass0gripids[fpid]):
sql = "INSERT IGNORE INTO ikassemblyxgrips0(idrobot, idarm, idassemblyxgrips0, feasibility, \
feasibility_assdir) VALUES (%d, %d, %d, '%s', '%s')" \
% (idrobot, idarm, idicoass0grip, self.icoass0gripsik[fpid][i],
self.icoass0gripsik_retassdir[fpid][i])
gdb.execute(sql)
# left arm
idarm = self.gdb.loadIdArm('lft')
for i, idicoass1grip in enumerate(self.icoass1gripids[fpid]):
sql = "INSERT IGNORE INTO ikassemblyxgrips1(idrobot, idarm, idassemblyxgrips1, feasibility, \
feasibility_assdir) VALUES (%d, %d, %d, '%s', '%s')" \
% (idrobot, idarm, idicoass1grip, self.icoass1gripsik[fpid][i],
self.icoass1gripsik_retassdir[fpid][i])
gdb.execute(sql)
def __loadIKFromDB(self, robot):
"""
load the feasibility of IK from db
:param robot:
:return:
author: weiwei
date: 20170307
"""
idrobot = self.gdb.loadIdRobot(robot)
self.assgikfeas0 = []
self.assgikfeas0_retassdir = []
self.assgikfeas1= []
self.assgikfeas1_retassdir = []
for fpid in range(len(self.gridsfloatingposemat4s)):
# right arm
assgikfeas0 = []
assgikfeas0_retassdir = []
idarm = self.gdb.loadIdArm('rgt')
for i, idassgrips in enumerate(self.icoass0gripids[fpid]):
assgikfeas0.append('False')
assgikfeas0_retassdir.append('False')
sql = "SELECT feasibility, feasibility_assdir FROM ikassemblyxgrips0 WHERE \
idrobot = %d AND idarm = %d and idassemblyxgrips0 = %d" % (idrobot, idarm, idassgrips)
result = self.gdb.execute(sql)
if len(result) != 0:
assgikfeas0[i] = result[0][0]
assgikfeas0_retassdir[i] = result[0][1]
else:
assert "Compute ik of the freeassemblygrips first!"
self.assgikfeas0.append(assgikfeas0)
self.assgikfeas0_retassdir.append(assgikfeas0_retassdir)
# left arm
assgikfeas1 = []
assgikfeas1_retassdir = []
idarm = self.gdb.loadIdArm('lft')
for i, idassgrips in enumerate(self.icoass1gripids[fpid]):
assgikfeas1.append('False')
assgikfeas1_retassdir.append('False')
sql = "SELECT feasibility, feasibility_assdir FROM ikassemblyxgrips1 WHERE \
idrobot = %d AND idarm = %d and idassemblyxgrips1 = %d" % (idrobot, idarm, idassgrips)
result = self.gdb.execute(sql)
if len(result) != 0:
assgikfeas1[i] = result[0][0]
assgikfeas1_retassdir[i] = result[0][1]
else:
assert "Compute ik of the freeassemblygrips first!"
self.assgikfeas1.append(assgikfeas1)
self.assgikfeas1_retassdir.append(assgikfeas1_retassdir)
def __genPandaRotmat4(self, icolevel=1, angles=[0, 45, 90, 135, 180, 225, 270, 315]):
"""
generate panda3d rotmat4 using icospheres and rotationaangle each origin-vertex vector of the icosphere
:param icolevel, the default value 1 = 42vertices
:param angles, 8 directions by default
:return: a list of [pandarotmat4, ...] size of the inner list is size of the angles
author: weiwei
date: 20170221, tsukuba (copied from regrasp/floatingposes.py)
"""
objnp = pg.genObjmnp(self.obj0path)
mat4list = []
self.icos = trimesh.creation.icosphere(icolevel)
initmat4 = objnp.getMat()
for vert in self.icos.vertices:
mat4list.append([])
objnp.lookAt(vert[0], vert[1], vert[2])
ytozmat4 = Mat4.rotateMat(-90, objnp.getMat().getRow3(0))
newobjmat4 = objnp.getMat() * ytozmat4
for angle in angles:
tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2))
tmppandamat4 = newobjmat4 * tmppandamat4
mat4list[-1].append(tmppandamat4)
objnp.setMat(initmat4)
self.floatingposemat4 = [e for l in mat4list for e in l]
def __genFreeAssGrips(self):
self.obj0grips = []
self.obj1grips = []
self.obj0grips, self.obj1grips = \
genAvailableFAGs(self.base, self.obj0path, self.obj0Mat4,
self.obj1path, self.obj1Mat4, self.gdb, self.handpkg)
def __transformGripsOnePose(self, objgrips, rotmat4):
"""
transform the freeair grips to one specific rotmat4
:param objgrips: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair]
:param rotmat4:
:return: [xassgripcontacts, xassgripnormals, xassgriprotmat4s, xassgripjawwidth, xassgripidfreeair]
each element in the list is also a list
author: weiwei
date: 20170307
"""
assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair = objgrips
xassgripcontacts = []
xassgripnormals = []
xassgriprotmat4s = []
xassgripjawwidth = []
xassgripidfreeair = []
for i, gripmat4 in enumerate(assgriprotmat4s):
floatinggripmat4 = gripmat4 * rotmat4
cct0 = rotmat4.xformPoint(assgripcontacts[i][0])
cct1 = rotmat4.xformPoint(assgripcontacts[i][1])
cctn0 = rotmat4.xformPoint(assgripnormals[i][0])
cctn1 = rotmat4.xformPoint(assgripnormals[i][1])
xassgripcontacts.append([cct0, cct1])
xassgripnormals.append([cctn0, cctn1])
xassgriprotmat4s.append(floatinggripmat4)
xassgripjawwidth.append(assgripjawwidth[i])
xassgripidfreeair.append(assgripidfreeair[i])
return [xassgripcontacts, xassgripnormals, xassgriprotmat4s, xassgripjawwidth, xassgripidfreeair]
def __transformGrips(self):
"""
transform the freeass grips to all rotmat4 in self.gridsfloatingposemat4s
:return:
author: weiwei
date: 20170221, tsukuba (copied from regrasp/floatingposes.py)
"""
self.icoass0gripcontacts = []
self.icoass0gripnormals = []
self.icoass0griprotmat4s = []
self.icoass0gripjawwidth = []
self.icoass0gripidfreeair = []
for icomat4 in self.gridsfloatingposemat4s:
xassgrips = self.__transformGripsOnePose(self.obj0grips, icomat4)
self.icoass0gripcontacts.append(xassgrips[0])
self.icoass0gripnormals.append(xassgrips[1])
self.icoass0griprotmat4s.append(xassgrips[2])
self.icoass0gripjawwidth.append(xassgrips[3])
self.icoass0gripidfreeair.append(xassgrips[4])
self.icoass1gripcontacts = []
self.icoass1gripnormals = []
self.icoass1griprotmat4s = []
self.icoass1gripjawwidth = []
self.icoass1gripidfreeair = []
for icomat4 in self.gridsfloatingposemat4s:
xassgrips = self.__transformGripsOnePose(self.obj1grips, icomat4)
self.icoass1gripcontacts.append(xassgrips[0])
self.icoass1gripnormals.append(xassgrips[1])
self.icoass1griprotmat4s.append(xassgrips[2])
self.icoass1gripjawwidth.append(xassgrips[3])
self.icoass1gripidfreeair.append(xassgrips[4])
class FloatingPoses(object):
"""
like freetabletopplacement and tableplacements
manipulation.floatingposes corresponds to freegrip
floatingposes doesn't take into account
the position and orientation of the object
it is "free" in position and rotation. No obstacles were considered.
In contrast, each item in regrasp.floatingposes
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 "freegrip"
"s" is attached to the end of "floatingposes"
meanings of abbreviations: FP = floating pose, G = grasps, GP = grasping pairs
author: weiwei
date: 2016, 2019
"""
def __init__(self, objpath, gdb, handpkg, base):
"""
initialization
:param objpath: path of the object
:param base: for the loadFreeAirGrip --> genHandPairs functions (for collision detection)
author: weiwei
date: 20161215, tsukuba
"""
self.__base = base
self.handpkg = handpkg
self.handname = handpkg.getHandName()
self.hand0 = handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
self.hand1 = handpkg.newHandNM(hndcolor=[0, 0, 1, .1])
self.objtrimesh = trimesh.load_mesh(objpath)
self.objnp = self.__base.pg.packpandanp_fn(
self.objtrimesh.vertices, self.objtrimesh.face_normals,
self.objtrimesh.faces)
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
# mat4list is nested list, floatingposemat4 is list (the flat version of mat4lsit), they are essentially the same
self.mat4list = []
self.floatingposemat4 = []
# gridsfloatingposemat4s is self.floatingposemat4 translated to different grids
self.gridsfloatingposemat4s = []
self.icos = None
self.floatinggripids = []
self.floatinggripmat4s = []
self.floatinggripcontacts = []
self.floatinggripnormals = []
self.floatinggripjawwidth = []
self.floatinggripidfreeair = []
self.bulletworld = BulletWorld()
self.handpairList = []
self.gdb = gdb
self.__loadFreeAirGrip(base)
# for IK
self.feasibility_rgt = []
self.feasibility_handa_rgt = []
self.feasibility_lft = []
self.feasibility_handa_lft = []
self.jnts_rgt = []
self.jnts_handa_rgt = []
self.jnts_lft = []
self.jnts_handa_lft = []
# for ik-feasible pairs
self.floatinggrippairsids = []
self.floatinggrippairshndmat4s = []
self.floatinggrippairscontacts = []
self.floatinggrippairsnormals = []
self.floatinggrippairsjawwidths = []
self.floatinggrippairsidfreeairs = []
self.floatinggrippairsjnts = []
self.floatinggrippairsjnts_handa = []
def __loadFreeAirGrip(self, base):
"""
load self.freegripids, 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.handpkg.getHandName())
if freeairgripdata is None:
raise ValueError("Plan the freeairgrip first!")
self.freegripids = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
def __genPandaRotmat4(self,
icolevel=1,
angles=[0, 45, 90, 135, 180, 225, 270, 315]):
"""
generate panda3d rotmat4 using icospheres and rotationaangle each origin-vertex vector of the icosphere
:param icolevel, the default value 1 = 42vertices
:param angles, 8 directions by default
:return: a list of [pandarotmat4, ...] size of the inner list is size of the angles
author: weiwei
date: 20170221, tsukuba
"""
self.mat4list = []
self.icos = trimesh.creation.icosphere(icolevel)
initmat4 = self.objnp.getMat()
for vert in self.icos.vertices:
self.mat4list.append([])
self.objnp.lookAt(vert[0], vert[1], vert[2])
ytozmat4 = Mat4.rotateMat(-90, self.objnp.getMat().getRow3(0))
newobjmat4 = self.objnp.getMat() * ytozmat4
for angle in angles:
tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2))
tmppandamat4 = newobjmat4 * tmppandamat4
self.mat4list[-1].append(tmppandamat4)
self.objnp.setMat(initmat4)
self.floatingposemat4 = [e for l in self.mat4list for e in l]
print(len(self.floatingposemat4))
def __checkFPexistance(self):
"""
check if the floating pose, grasps, and the handover pairs already exist
if exist, return True
else return False
:return: boolean value indicating the existance of floating poses, grasps, and the handover pairs
author: weiwei
date: 20190318
"""
sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) > 0:
print("Floating poses already saved!")
isredo = input("Do you want to overwrite the database? (Y/N)")
if isredo != "Y" and isredo != "y":
print("Floating pose planning aborted.")
return True
else:
print("Deleting existing floating poses...")
sql = "DELETE FROM floatingposes USING floatingposes, object WHERE floatingposes.idobject = object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
self.gdb.execute(sql)
return False
def __saveFPGToDB(self):
"""
save the floatingposes and their grasps to the database
:return:
author: weiwei
date: 20170221
"""
print("Saving floating poses and grasps into the database...")
sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) == 0:
# the gridsfloatingposes for the self.dbobjname is not saved
sql = "INSERT INTO floatingposes(rotmat, idobject) VALUES "
for i in range(len(self.gridsfloatingposemat4s)):
# print i, "/", len(self.gridsfloatingposemat4s)
sql += "('%s', (SELECT idobject FROM object WHERE name LIKE '%s')), " % \
(dc.mat4ToStr(self.gridsfloatingposemat4s[i]), self.dbobjname)
sql = sql[:-2] + ";"
self.gdb.execute(sql)
sql = "SELECT * FROM floatinggrips,floatingposes,object,freeairgrip,hand WHERE \
floatinggrips.idfloatingposes = floatingposes.idfloatingposes AND \
floatingposes.idobject = object.idobject AND \
object.name LIKE '%s' AND floatinggrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = hand.idhand AND hand.name LIKE '%s'" % (
self.dbobjname, self.handpkg.getHandName())
result = self.gdb.execute(sql)
if len(result) == 0:
tic = time.time()
for i in range(len(self.gridsfloatingposemat4s)):
toc = time.time()
print(
"Saving FP and Gs to DB", " Finished: ", i, "/",
len(self.gridsfloatingposemat4s), " Time past: ",
"%.2f" % (toc - tic), "s", " Expected remaining time: ",
"%.2f" % ((toc - tic) * len(self.gridsfloatingposemat4s) /
(i + 1e-4) - (toc - tic)), "s")
sql = "SELECT floatingposes.idfloatingposes FROM floatingposes,object WHERE \
floatingposes.idobject = object.idobject AND \
floatingposes.rotmat LIKE '%s' AND \
object.name LIKE '%s'" % (dc.mat4ToStr(
self.gridsfloatingposemat4s[i]), self.dbobjname)
result = self.gdb.execute(sql)[0]
if len(result) != 0:
idfloatingposes = result[0]
sql = "SELECT * FROM floatinggrips WHERE idfloatingposes = %d" % idfloatingposes
result = self.gdb.execute(sql)
if len(result) == 0:
if len(self.floatinggripmat4s[i]) != 0:
sql = "INSERT INTO floatinggrips(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfloatingposes, idfreeairgrip) VALUES "
for j in range(len(self.floatinggripmat4s[i])):
# print "gripposes", i, "/", len(self.gridsfloatingposemat4s)
# print "grips", j, "/", len(self.floatinggripmat4s[i])
cct0 = self.floatinggripcontacts[i][j][0]
cct1 = self.floatinggripcontacts[i][j][1]
cctn0 = self.floatinggripnormals[i][j][0]
cctn1 = self.floatinggripnormals[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.floatinggripmat4s[i][j]), str(self.floatinggripjawwidth[i][j]), \
idfloatingposes, self.floatinggripidfreeair[i][j])
sql = sql[:-2] + ";"
self.gdb.execute(sql)
def __genHandPairs(self, base, loadser=False):
self.handpairList = []
if loadser is False:
# hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
# hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
pairidlist = list(
itertools.combinations(range(len(self.freegripids)), 2))
total = len(pairidlist) / (len(pairidlist) / 5000) + 1
tic = time.time()
for i in range(100, len(pairidlist),
int(len(pairidlist) / 5000.0 + 1)):
toc = time.time()
print(
"Generating grasp pairs", " Finished: ", "%.2f" %
(i / int(len(pairidlist) / 5000.0 + 1) / total * 100), "%",
" Time past: ", "%.2f" % (toc - tic), "s",
" Expected remaining time: ", "%.2f" %
((toc - tic) * int(len(pairidlist) / 5000.0 + 1) * total /
(i + 1e-4) - (toc - tic)), "s")
i0, i1 = pairidlist[i]
self.hand0.setMat(pandanpmat4=self.freegriprotmats[i0])
# self.hand0.setMat(npmat4=self.freegriprotmats[i0])
self.hand0.setJawwidth(self.freegripjawwidth[i0])
self.hand1.setMat(pandanpmat4=self.freegriprotmats[i1])
# self.hand1.setMat(npmat4=self.freegriprotmats[i1])
self.hand1.setJawwidth(self.freegripjawwidth[i1])
hndbullnodei0 = cd.genBulletCDMeshMultiNp(
self.hand0.handnp, base.render)
hndbullnodei1 = cd.genBulletCDMeshMultiNp(
self.hand1.handnp, base.render)
result = self.bulletworld.contactTestPair(
hndbullnodei0, hndbullnodei1)
if not result.getNumContacts():
# check the open state
self.hand0.setJawwidth(self.hand0.jawwidthopen)
self.hand1.setJawwidth(self.hand1.jawwidthopen)
hndbullnodei0 = cd.genBulletCDMeshMultiNp(
self.hand0.handnp, base.render)
hndbullnodei1 = cd.genBulletCDMeshMultiNp(
self.hand1.handnp, base.render)
result = self.bulletworld.contactTestPair(
hndbullnodei0, hndbullnodei1)
if not result.getNumContacts():
self.handpairList.append(
[self.freegripids[i0], self.freegripids[i1]])
json.dump(self.handpairList, open("tmp.json", mode="w"))
else:
self.handpairList = json.load(open("tmp.json", mode="r"))
def __transformGrips(self):
"""
transform the freeair grips to all rotmat4 in self.gridsfloatingposemat4s
:return:
author: weiwei
date: 20170221, tsukuba
"""
self.floatinggripmat4s = []
self.floatinggripcontacts = []
self.floatinggripnormals = []
self.floatinggripjawwidth = []
self.floatinggripidfreeair = []
for icomat4 in self.gridsfloatingposemat4s:
floatinggrips = self.transformGripsOnePose(icomat4)
self.floatinggripmat4s.append(floatinggrips[0])
self.floatinggripcontacts.append(floatinggrips[1])
self.floatinggripnormals.append(floatinggrips[2])
self.floatinggripjawwidth.append(floatinggrips[3])
self.floatinggripidfreeair.append(floatinggrips[4])
def __updateDBwithGP(self, loadser=False):
"""
compute the floatinggrippairs using freegrippairs and
save the floatinggripspairs into Database
:param loadser whether use serialized data for handpairlist
:return:
author: weiwei
date: 20170301
"""
print(
"Start updating the floatingpose/grasps DB with floating grasp pairs!"
)
if len(self.handpairList) == 0:
self.__genHandPairs(base, loadser)
tic = time.time()
for fpid in range(len(self.gridsfloatingposemat4s)):
toc = time.time()
print(
"Saving grasp pairs", " Finished: ", fpid, "/",
len(self.gridsfloatingposemat4s), " Time past: ",
"%.2f" % (toc - tic), "s", " Expected remaining time: ",
"%.2f" % ((toc - tic) * len(self.gridsfloatingposemat4s) /
(fpid + 1e-4) - (toc - tic)), "s")
# gen correspondence between freeairgripid and index
# indfgoffa means index of floatinggrips whose freeairgripid are xxx
indfgoffa = {}
for i in range(len(self.floatinggripidfreeair[fpid])):
indfgoffa[self.floatinggripidfreeair[fpid][i]] = i
# handidpair_indfg is the pairs using index of floatinggrips
handidpair_indfg = []
sql = "INSERT INTO floatinggripspairs VALUES "
for handidpair in self.handpairList:
handidpair_indfg.append(
[indfgoffa[handidpair[0]], indfgoffa[handidpair[1]]])
# if handidpair_indfg[0] is right, 1 is left
sql = sql + "(%d, %d)" % \
(self.floatinggripids[fpid][handidpair_indfg[-1][0]], self.floatinggripids[fpid][handidpair_indfg[-1][1]])
# if 1 is right, 0 is left
sql = sql + ", (%d, %d), " % \
(self.floatinggripids[fpid][handidpair_indfg[-1][1]], self.floatinggripids[fpid][handidpair_indfg[-1][0]])
# self.gdb.execute(sql)
# # if 1 is right, 0 is left
# sql = "INSERT INTO floatinggripspairs VALUES (%d, %d)" % \
# (self.floatinggripids[fpid][handidpair_indfg[-1][1]], self.floatinggripids[fpid][handidpair_indfg[-1][0]])
sql = sql[:-2]
self.gdb.execute(sql)
def __checkIKexistance(self, idrobot):
"""
check if the ik has been computed
the existance is confirmed if the ik of the first idfloatinggrips has been updated
:param idrobot:
:return: boolean value indicating the existence of ik, True for existence, False for non-existence
author: weiwei
date: 20190318
"""
# bexist = False
# for fpid in range(len(self.gridsfloatingposemat4s)):
# print(fpid, len(self.gridsfloatingposemat4s))
# # right arm
# idarm = self.gdb.loadIdArm('rgt')
# for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
# sql = "SELECT * FROM ikfloatinggrips WHERE idrobot LIKE '%s' AND idarm LIKE '%s' \
# AND idfloatinggrips LIKE '%s'" % (idrobot, idarm, idfloatinggrips)
# result = self.gdb.execute(sql)
# if(len(result)>0):
# bexist = True
# break
# if bexist:
# break
# # left arm
# idarm = self.gdb.loadIdArm('lft')
# for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
# sql = "SELECT * FROM ikfloatinggrips WHERE idrobot LIKE '%s' AND idarm LIKE '%s' \
# AND idfloatinggrips LIKE '%s'" % (idrobot, idarm, idfloatinggrips)
# result = self.gdb.execute(sql)
# if(len(result)>0):
# bexist = True
# break
# if bexist:
# break
#
# if bexist:
# print("IK is already updated!")
# isredo = input("Do you want to overwrite the database? (Y/N)")
# if isredo != "Y" and isredo != "y":
# print("Updating IK aborted.")
# return True
# else:
# for fpid in range(len(self.gridsfloatingposemat4s)):
# # right arm
# idarm = self.gdb.loadIdArm('rgt')
# for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
# sql = "DELETE FROM ikfloatinggrips WHERE idrobot LIKE '%s' AND idarm LIKE '%s' \
# AND idfloatinggrips LIKE '%s'" % (idrobot, idarm, idfloatinggrips)
# self.gdb.execute(sql)
# # left arm
# idarm = self.gdb.loadIdArm('lft')
# for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
# sql = "DELETE FROM ikfloatinggrips WHERE idrobot LIKE '%s' AND idarm LIKE '%s' \
# AND idfloatinggrips LIKE '%s'" % (idrobot, idarm, idfloatinggrips)
# self.gdb.execute(sql)
# return False
# else:
# return False
sql = "SELECT * FROM ikfloatinggrips WHERE idrobot LIKE '%s'" % (
idrobot)
result = self.gdb.execute(sql)
if (len(result) > 0):
print("IK is already updated!")
isredo = input("Do you want to overwrite the database? (Y/N)")
if isredo != "Y" and isredo != "y":
print("Updating IK aborted.")
return True
else:
sql = "DELETE FROM ikfloatinggrips WHERE idrobot LIKE '%s'" % (
idrobot)
self.gdb.execute(sql)
return False
else:
return False
def __saveIKtoDB(self, idrobot):
"""
saveupdated IK to DB
this function is separated from updateDBwithIK for illustration
:return:
author: weiwei
date: 20190316, toyonaka
"""
for fpid in range(len(self.gridsfloatingposemat4s)):
# right arm
idarm = self.gdb.loadIdArm('rgt')
for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
sql = "INSERT INTO ikfloatinggrips(idrobot, idarm, idfloatinggrips, feasibility, \
feasibility_handa, jnts, jnts_handa) VALUES (%d, %d, %d, '%s', '%s', '%s', '%s')" \
% (idrobot, idarm, idfloatinggrips, self.feasibility_rgt[fpid][i],
self.feasibility_handa_rgt[fpid][i], self.jnts_rgt[fpid][i], self.jnts_handa_rgt[fpid][i])
self.gdb.execute(sql)
# left arm
idarm = self.gdb.loadIdArm('lft')
for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
sql = "INSERT INTO ikfloatinggrips(idrobot, idarm, idfloatinggrips, feasibility, \
feasibility_handa, jnts, jnts_handa) VALUES (%d, %d, %d, '%s', '%s', '%s', '%s')" \
% (idrobot, idarm, idfloatinggrips, self.feasibility_lft[fpid][i],
self.feasibility_handa_lft[fpid][i], self.jnts_lft[fpid][i], self.jnts_handa_lft[fpid][i])
self.gdb.execute(sql)
def genFPGPandSaveToDB(self,
grids,
icolevel=1,
angles=[0, 45, 90, 135, 180, 225, 270, 315]):
"""
genterate floating poses and their grasps and the floatinggrippairs,
this function leverages genPandaRotmat4 and transformGrips
:param icolevel
:param angles see genPandaRotmat4
:return:
author: weiwei
date: 20170221
"""
if self.__checkFPexistance():
pass
else:
self.gridsfloatingposemat4s = []
self.__genPandaRotmat4(icolevel, angles)
for gridposition in grids:
for posemat4 in self.floatingposemat4:
tmpposemat4 = Mat4(posemat4)
tmpposemat4.setRow(
3,
Vec3(gridposition[0], gridposition[1],
gridposition[2]))
self.gridsfloatingposemat4s.append(tmpposemat4)
self.__transformGrips()
print("Saving the floatingposes and grasps to database")
self.__saveFPGToDB()
print("Updating the databse with floating grasppairs")
self.loadFPGfromDB() # update floatinggripids
self.__updateDBwithGP()
def updateDBwithIK(self, robot):
"""
compute the IK feasible grasps of each pose
:return:
author: weiwei
date: 20190316
"""
# load retraction distances
rethanda, retworlda, worldz = self.gdb.loadIKRet()
# load idrobot
idrobot = self.gdb.loadIdRobot(robot)
if self.__checkIKexistance(idrobot):
pass
else:
self.feasibility_rgt = []
self.feasibility_handa_rgt = []
self.feasibility_lft = []
self.feasibility_handa_lft = []
self.jnts_rgt = []
self.jnts_handa_rgt = []
self.jnts_lft = []
self.jnts_handa_lft = []
tic = time.time()
for fpid in range(len(self.gridsfloatingposemat4s)):
toc = time.time()
print(
"Updating IK", " Finished: ", fpid, "/",
len(self.gridsfloatingposemat4s), " Time past: ",
"%.2f" % (toc - tic), "s", " Expected remaining time: ",
"%.2f" % ((toc - tic) * len(self.gridsfloatingposemat4s) /
(fpid + 1e-4) - (toc - tic)), "s")
### right hand
armid = 'rgt'
feasibility = []
feasibility_handa = []
jnts = []
jnts_handa = []
for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]):
feasibility.append('False')
feasibility_handa.append('False')
jnts.append([])
jnts_handa.append([])
fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0] +
self.floatinggripcontacts[fpid][i][1]) / 2
fpgsfgrcenternp = self.__base.pg.v3ToNp(fpgsfgrcenter)
fpgsrotmat3np = self.__base.pg.mat3ToNp(
hndrotmat4.getUpper3())
handa = -hndrotmat4.getRow3(2)
# check the angle between handa and minus y
minusworldy = Vec3(0, -1, 0)
if Vec3(handa).angleDeg(minusworldy) < 60:
msc = robot.numik(fpgsfgrcenternp, fpgsrotmat3np,
armid)
if msc is not None:
feasibility[-1] = 'True'
jnts[-1] = dc.listToStr(msc)
fpgsfgrcenternp_handa = self.__base.pg.v3ToNp(
fpgsfgrcenter + handa * rethanda)
msc_handa = robot.numikmsc(fpgsfgrcenternp_handa,
fpgsrotmat3np, msc,
armid)
if msc_handa is not None:
feasibility_handa[-1] = 'True'
jnts_handa[-1] = dc.listToStr(msc_handa)
self.feasibility_rgt.append(feasibility)
self.feasibility_handa_rgt.append(feasibility_handa)
self.jnts_rgt.append(jnts)
self.jnts_handa_rgt.append(jnts_handa)
### left hand
armid = 'lft'
feasibility = []
feasibility_handa = []
jnts = []
jnts_handa = []
for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]):
feasibility.append('False')
feasibility_handa.append('False')
jnts.append([])
jnts_handa.append([])
fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0] +
self.floatinggripcontacts[fpid][i][1]) / 2
fpgsfgrcenternp = self.__base.pg.v3ToNp(fpgsfgrcenter)
fpgsrotmat3np = self.__base.pg.mat3ToNp(
hndrotmat4.getUpper3())
handa = -hndrotmat4.getRow3(2)
# check the angle between handa and minus y
plusworldy = Vec3(0, 1, 0)
if Vec3(handa).angleDeg(plusworldy) < 60:
msc = robot.numik(fpgsfgrcenternp, fpgsrotmat3np,
armid)
if msc is not None:
feasibility[-1] = 'True'
jnts[-1] = dc.listToStr(msc)
fpgsfgrcenternp_handa = self.__base.pg.v3ToNp(
fpgsfgrcenter + handa * rethanda)
msc_handa = robot.numikmsc(fpgsfgrcenternp_handa,
fpgsrotmat3np, msc,
armid)
if msc_handa is not None:
feasibility_handa[-1] = 'True'
jnts_handa[-1] = dc.listToStr(msc_handa)
self.feasibility_lft.append(feasibility)
self.feasibility_handa_lft.append(feasibility_handa)
self.jnts_lft.append(jnts)
self.jnts_handa_lft.append(jnts_handa)
self.__saveIKtoDB(idrobot)
print("IK is updated!")
def loadFPGfromDB(self):
"""
for debug purpose
load the floatingposes and their grasps from the database
:return:
author: weiwei
date: 20170227
"""
print("Loading FP and Gs to update floatinggripids...")
self.gridsfloatingposemat4s = []
self.floatinggripids = []
self.floatinggripmat4s = []
self.floatinggripcontacts = []
self.floatinggripnormals = []
self.floatinggripjawwidth = []
self.floatinggripidfreeair = []
sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
fposesresult = self.gdb.execute(sql)
if len(fposesresult) != 0:
tic = time.time()
for i, resultrow in enumerate(fposesresult):
toc = time.time()
print(
"Loading floatinggripids", " Finished: ", i, "/",
len(fposesresult), " Time past: ", "%.2f" % (toc - tic),
"s", " Expected remaining time: ",
"%.2f" % ((toc - tic) * len(fposesresult) / (i + 1e-4) -
(toc - tic)), "s")
self.gridsfloatingposemat4s.append(dc.strToMat4(resultrow[1]))
idfloatingposes = resultrow[0]
sql = "SELECT floatinggrips.idfloatinggrips, floatinggrips.contactpoint0, \
floatinggrips.contactpoint1, floatinggrips.contactnormal0, \
floatinggrips.contactnormal1, floatinggrips.rotmat, floatinggrips.jawwidth, \
floatinggrips.idfreeairgrip FROM floatinggrips,freeairgrip,hand WHERE \
floatinggrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = hand.idhand AND floatinggrips.idfloatingposes = %d AND \
hand.name = '%s'" % (idfloatingposes,
self.handpkg.getHandName())
fgresult = self.gdb.execute(sql)
if len(fgresult) != 0:
floatinggripids = []
floatinggripmat4s = []
floatinggripcontacts = []
floatinggripnormals = []
floatinggripjawwidths = []
floatinggripidfreeairs = []
for fgresultrow in fgresult:
cct0 = dc.strToV3(fgresultrow[1])
cct1 = dc.strToV3(fgresultrow[2])
cctn0 = dc.strToV3(fgresultrow[3])
cctn1 = dc.strToV3(fgresultrow[4])
floatinggripids.append(int(fgresultrow[0]))
floatinggripmat4s.append(dc.strToMat4(fgresultrow[5]))
floatinggripcontacts.append([cct0, cct1])
floatinggripnormals.append([cctn0, cctn1])
floatinggripjawwidths.append(float(fgresultrow[6]))
floatinggripidfreeairs.append(int(fgresultrow[7]))
self.floatinggripids.append(floatinggripids)
self.floatinggripmat4s.append(floatinggripmat4s)
self.floatinggripcontacts.append(floatinggripcontacts)
self.floatinggripnormals.append(floatinggripnormals)
self.floatinggripjawwidth.append(floatinggripjawwidths)
self.floatinggripidfreeair.append(floatinggripidfreeairs)
else:
print('Plan floating grips first!')
assert (False)
else:
assert ('No object found!')
def transformGripsOnePose(self, rotmat4):
"""
transform the freeair grips to one specific rotmat4
:param rotmat4:
:return: [floatinggripmat4s, floatinggripcontacts, floatinggripnormals, floatinggripjawwidths, floatinggripidfreeairs]
each element in the list is also a list
"""
floatinggripmat4s = []
floatinggripcontacts = []
floatinggripnormals = []
floatinggripjawwidths = []
floatinggripidfreeairs = []
for i, gripmat4 in enumerate(self.freegriprotmats):
floatinggripmat4 = gripmat4 * rotmat4
cct0 = rotmat4.xformPoint(self.freegripcontacts[i][0])
cct1 = rotmat4.xformPoint(self.freegripcontacts[i][1])
cctn0 = rotmat4.xformPoint(self.freegripnormals[i][0])
cctn1 = rotmat4.xformPoint(self.freegripnormals[i][1])
floatinggripjawwidth = self.freegripjawwidth[i]
floatinggripidfreeair = self.freegripids[i]
floatinggripmat4s.append(floatinggripmat4)
floatinggripcontacts.append([cct0, cct1])
floatinggripnormals.append([cctn0, cctn1])
floatinggripjawwidths.append(floatinggripjawwidth)
floatinggripidfreeairs.append(floatinggripidfreeair)
return [
floatinggripmat4s, floatinggripcontacts, floatinggripnormals,
floatinggripjawwidths, floatinggripidfreeairs
]
def showIcomat4s(self, nodepath):
"""
show the pandamat4s generated by genPandaRotmat4
:param nodepath, where np to repreantTo, usually base.render
:return:
author: weiwei
date: 20170221, tsukuba
"""
for i, icomat4list in enumerate(self.mat4list):
vert = self.icos.vertices * 100
spos = Vec3(vert[i][0], vert[i][1], vert[i][2])
for icomat4 in icomat4list:
self.__base.pg.plotAxis(nodepath,
spos,
icomat4,
length=100,
thickness=3)
def loadIKfeasibleGPfromDB(self, robot):
"""
load the IK Feasible handover pairs
:return:
author: weiwei
date: 20170301
"""
self.loadFPGfromDB()
self.loadIKFromDB(robot)
idrobot = self.gdb.loadIdRobot(robot)
idarmrgt = self.gdb.loadIdArm('rgt')
idarmlft = self.gdb.loadIdArm('lft')
self.floatinggrippairsids = []
self.floatinggrippairshndmat4s = []
self.floatinggrippairscontacts = []
self.floatinggrippairsnormals = []
self.floatinggrippairsjawwidths = []
self.floatinggrippairsidfreeairs = []
self.floatinggrippairsjnts = []
self.floatinggrippairsjnts_handa = []
for fpid in range(len(self.gridsfloatingposemat4s)):
sql = "SELECT floatingposes.idfloatingposes FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s' AND floatingposes.rotmat LIKE '%s'" % \
(self.dbobjname, dc.mat4ToStr(self.gridsfloatingposemat4s[fpid]))
result = self.gdb.execute(sql)
floatinggrippairsids = []
floatinggrippairshndmat4s = []
floatinggrippairscontacts = []
floatinggrippairsnormals = []
floatinggrippairsjawwidths = []
floatinggrippairsidfreeairs = []
floatinggrippairsjnts = []
floatinggrippairsjnts_handa = []
if len(result) != 0:
idfloatingposes = result[0][0]
sql = "SELECT floatinggripspairs.idfloatinggrips0, floatinggripspairs.idfloatinggrips1, \
fg0.contactpoint0, fg0.contactpoint1, fg0.contactnormal0, fg0.contactnormal1, fg0.rotmat, \
fg0.jawwidth, fg0.idfreeairgrip, \
fg1.contactpoint0, fg1.contactpoint1, fg1.contactnormal0, fg1.contactnormal1, fg1.rotmat, \
fg1.jawwidth, fg1.idfreeairgrip, ikfg0.jnts, ikfg0.jnts_handa, \
ikfg1.jnts, ikfg1.jnts_handa FROM floatinggripspairs, floatinggrips fg0, floatinggrips fg1, \
ikfloatinggrips ikfg0, ikfloatinggrips ikfg1 WHERE \
floatinggripspairs.idfloatinggrips0 = fg0.idfloatinggrips AND \
floatinggripspairs.idfloatinggrips1 = fg1.idfloatinggrips AND \
fg0.idfloatingposes = %d AND fg1.idfloatingposes = %d AND \
fg0.idfloatinggrips = ikfg0.idfloatinggrips AND \
ikfg0.feasibility like 'True' AND ikfg0.feasibility_handa like 'True' AND \
ikfg0.idrobot = %d AND ikfg0.idarm = %d AND \
fg1.idfloatinggrips = ikfg1.idfloatinggrips AND ikfg1.feasibility like 'True' \
AND ikfg1.feasibility_handa like 'True' AND \
ikfg1.idrobot = %d AND ikfg1.idarm = %d" % \
(idfloatingposes, idfloatingposes, idrobot, idarmrgt, idrobot, idarmlft)
result = self.gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
floatinggrippairsids.append(
[resultrow[0], resultrow[1]])
floatinggrippairshndmat4s.append([
dc.strToMat4(resultrow[6]),
dc.strToMat4(resultrow[13])
])
rgtcct0 = dc.strToV3(resultrow[2])
rgtcct1 = dc.strToV3(resultrow[3])
lftcct0 = dc.strToV3(resultrow[9])
lftcct1 = dc.strToV3(resultrow[10])
floatinggrippairscontacts.append([[rgtcct0, rgtcct1],
[lftcct0, lftcct1]])
rgtcctn0 = dc.strToV3(resultrow[4])
rgtcctn1 = dc.strToV3(resultrow[5])
lftcctn0 = dc.strToV3(resultrow[11])
lftcctn1 = dc.strToV3(resultrow[12])
floatinggrippairsnormals.append([[rgtcctn0, rgtcctn1],
[lftcctn0, lftcctn1]])
floatinggrippairsjawwidths.append(
[float(resultrow[7]),
float(resultrow[14])])
floatinggrippairsidfreeairs.append(
[int(resultrow[8]),
int(resultrow[15])])
floatinggrippairsjnts.append([
dc.strToList(resultrow[16]),
dc.strToList(resultrow[18])
])
floatinggrippairsjnts_handa.append([
dc.strToList(resultrow[17]),
dc.strToList(resultrow[19])
])
self.floatinggrippairsids.append(floatinggrippairsids)
self.floatinggrippairshndmat4s.append(floatinggrippairshndmat4s)
self.floatinggrippairscontacts.append(floatinggrippairscontacts)
self.floatinggrippairsnormals.append(floatinggrippairsnormals)
self.floatinggrippairsjawwidths.append(floatinggrippairsjawwidths)
self.floatinggrippairsidfreeairs.append(
floatinggrippairsidfreeairs)
self.floatinggrippairsjnts.append(floatinggrippairsjnts)
self.floatinggrippairsjnts_handa.append(
floatinggrippairsjnts_handa)
def loadIKFromDB(self, robot):
"""
load the feasibility of IK from db
:param robot:
:return:
author: weiwei
date: 20170301
"""
idrobot = self.gdb.loadIdRobot(robot)
self.feasibility_rgt = []
self.feasibility_handa_rgt = []
self.feasibility_lft = []
self.feasibility_handa_lft = []
for fpid in range(len(self.gridsfloatingposemat4s)):
# right arm
feasibility = []
feasibility_handa = []
idarm = self.gdb.loadIdArm('rgt')
for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
feasibility.append('False')
feasibility_handa.append('False')
sql = "SELECT feasibility, feasibility_handa FROM ikfloatinggrips WHERE \
idrobot = %d AND idarm = %d and idfloatinggrips = %d" % (
idrobot, idarm, idfloatinggrips)
result = self.gdb.execute(sql)
if len(result) != 0:
feasibility[i] = result[0][0]
feasibility_handa[i] = result[0][1]
self.feasibility_rgt.append(feasibility)
self.feasibility_handa_rgt.append(feasibility_handa)
# left arm
feasibility = []
feasibility_handa = []
idarm = self.gdb.loadIdArm('lft')
for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]):
feasibility.append('False')
feasibility_handa.append('False')
sql = "SELECT feasibility, feasibility_handa FROM ikfloatinggrips WHERE \
idrobot = %d AND idarm = %d and idfloatinggrips = %d" % (
idrobot, idarm, idfloatinggrips)
result = self.gdb.execute(sql)
if len(result) != 0:
feasibility[i] = result[0][0]
feasibility_handa[i] = result[0][1]
self.feasibility_lft.append(feasibility)
self.feasibility_handa_lft.append(feasibility_handa)
def plotOneFPandG(self, parentnp, fpid=0):
"""
plot the objpose and grasps at a specific rotmat4
:param fpid: the index of self.floatingposemat4
:return:
author: weiwei
date: 20170221, tsukuba
"""
objnp = self.__base.pg.packpandanp_fn(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
objnp.setMat(self.gridsfloatingposemat4s[fpid])
objnp.reparentTo(parentnp)
for i, hndrotmat4 in enumerate(self.gridsfloatingposemat4s[fpid]):
if i == 7:
# show grasps
hand0 = self.handpkg.newHandNM(hndcolor=[0, 1, 0, 1])
hand0.setMat(pandanpmat4=hndrotmat4)
hand0.setJawwidth(self.floatinggripjawwidth[fpid][i])
hand0.reparentTo(parentnp)
print(self.handpairList)
for handidpair in self.handpairList:
if handidpair[0] == self.floatinggripidfreeair[fpid][i]:
pairedilist = [
i1 for i1 in range(
len(self.floatinggripidfreeair[fpid]))
if self.floatinggripidfreeair[fpid][i1] ==
handidpair[1]
]
print(pairedilist)
i1 = pairedilist[0]
# if self.feasibility_lft[fpid][i1] == 'True':
hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1])
hndrotmat4 = self.floatinggripmat4s[fpid][i1]
hand1.setMat(pandanpmat4=hndrotmat4)
hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1])
hand1.reparentTo(parentnp)
if handidpair[1] == self.floatinggripidfreeair[fpid][i]:
pairedilist = [
i1 for i1 in range(
len(self.floatinggripidfreeair[fpid]))
if self.floatinggripidfreeair[fpid][i1] ==
handidpair[0]
]
print(pairedilist)
i1 = pairedilist[0]
# if self.feasibility_lft[fpid][i1] == 'True':
hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1])
hndrotmat4 = self.floatinggripmat4s[fpid][i1]
hand1.setMat(pandanpmat4=hndrotmat4)
hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1])
hand1.reparentTo(parentnp)
def plotOneFPandGPairs(self, parentnp, fpid=0):
"""
plot the gpairss at a specific rotmat4
:param fpid: the index of self.floatingposemat4
:return:
author: weiwei
date: 20170301, tsukuba
"""
objnp = self.__base.pg.packpandanp_fn(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
objnp.setMat(self.gridsfloatingposemat4s[fpid])
objnp.setColor(Vec4(.7, 0.3, 0, 1))
objnp.reparentTo(parentnp)
print(self.floatinggrippairshndmat4s[fpid])
for i, hndrotmat4pair in enumerate(
self.floatinggrippairshndmat4s[fpid]):
# if i == 9:
# show grasps
hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .5])
hand0mat4 = Mat4(hndrotmat4pair[0])
# h0row3 = hand0mat4.getRow3(3)
# h0row3[2] = h0row3[2]+i*20.0
# hand0mat4.setRow(3, h0row3[2])
hand0.setMat(pandanpmat4=hand0mat4)
hand0.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][0])
hand0.reparentTo(parentnp)
hand1 = self.handpkg.newHandNM(hndcolor=[0, .0, 1, .5])
hand1mat4 = Mat4(hndrotmat4pair[1])
# h1row3 = hand1mat4.getRow3(3)
# h1row3[2] = h1row3[2]+i*20.0
# hand1mat4.setRow(3, h1row3[2])
hand1.setMat(pandanpmat4=hand1mat4)
hand1.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][1])
hand1.reparentTo(parentnp)
class EccoGame(ShowBase):
def __init__(self):
ShowBase.__init__(self)
base.setBackgroundColor(0, 0, 0)
game_title = "ECCO"
titleN = TextNode('game-title')
font = loader.loadFont("font/Caveman.ttf")
titleN.setFont(font)
titleN.setText(game_title)
titleN.setTextColor(1, 1, 1, 1)
titleN.setSlant(0.1)
titleN.setShadow(0.05)
titleN.setShadowColor(0, 0, 200, 1)
titleN.setFrameColor(0, 0, 255, 1)
titleN.setFrameLineWidth(5.0)
textNodePath = self.aspect2d.attachNewNode(titleN)
textNodePath.setPos(-0.4, 1.5, 0.5)
textNodePath.setScale(0.2)
self.level1Button = DirectButton(text=("Level 1"), scale=.1, pos=(0, 0, 0.2), command=self.level1)
self.level2Button = DirectButton(text=("Level 2"), scale=.1, pos=(0, 0, 0), command=self.level2)
def level1(self):
titleNp = self.aspect2d.find('game-title')
titleNp.removeNode()
self.level1Button.destroy()
self.level2Button.destroy()
self.sizescale = 0.6
self.setupWorld()
self.setupSky()
self.setupFloor()
self.setupCharacter()
self.inst1 = addInstructions(0.95, "[ESC]: Quit")
self.inst2 = addInstructions(0.90, "[Left key]: Turn Ecco Left")
self.inst3 = addInstructions(0.85, "[Right key]: Turn Ecco Right")
self.inst4 = addInstructions(0.80, "[Up key]: Jump Ecco")
inputState.watchWithModifiers('esc', 'escape')
inputState.watchWithModifiers('w', 'w')
inputState.watchWithModifiers('arrow_left', 'arrow_left')
inputState.watchWithModifiers('arrow_right', 'arrow_right')
inputState.watchWithModifiers('pause', 'p')
inputState.watchWithModifiers('space', 'space')
inputState.watchWithModifiers('arrow_up', 'arrow_up')
inputState.watchWithModifiers('cam-left', 'z')
inputState.watchWithModifiers('cam-right', 'x')
inputState.watchWithModifiers('cam-forward', 'c')
inputState.watchWithModifiers('cam-backward', 'v')
taskMgr.add(self.update, "update")
# Game state variables
self.isMoving = False
# display framerate
self.setFrameRateMeter(True)
# Set up the camera
self.disableMouse()
self.camera.setPos(self.characterNP.getX(), self.characterNP.getY() - 30, 4)
self.setupSound()
# coins variables
self.coinsCollected = 0
self.dictOfCoins = {}
self.coins = []
# Set up Coins as Collectables
self.setupCoins()
# Set up Obstacles
self.setupObstacles()
# Setup Level Display
self.setupLevelDisplay()
self.counter = 0
def setupLevelDisplay(self):
LEVEL_1 = "Level 1"
levelDisplay(LEVEL_1)
levelN = TextNode('level-display')
levelN.setText(LEVEL_1)
font = loader.loadFont("font/Caveman.ttf")
levelN.setFont(font)
levelN.setTextColor(1, 1, 1, 1)
levelN.setSlant(0.1)
levelN.setShadow(0.05)
levelN.setShadowColor(255, 0, 0, 1)
textNodePath = self.aspect2d.attachNewNode(levelN)
textNodePath.setPos(-0.45, 0, 0)
textNodePath.setScale(0.2)
def update(self, task):
dt = globalClock.getDt()
self.pos = self.characterNP.getPos()
self.counter = self.counter + 1
if self.counter == 150:
levelNp = self.aspect2d.find('level-display')
levelNp.removeNode()
self.setUpCamera()
self.processInput(dt)
self.processContacts()
self.coinScoreDisplay()
self.checkIfEccoDied()
self.world.doPhysics(dt, 10, 1 / 230.0)
return task.cont
def setupWorld(self):
# create bullet world
self.debugNP = self.render.attachNewNode(BulletDebugNode('Debug'))
#self.debugNP.show()
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
def setupSky(self):
self.milkyWayNp = render.attachNewNode('milkyway')
self.milkyWay_2Np = render.attachNewNode('milkyway_2')
self.marsNp = render.attachNewNode('mars')
self.sunNp = render.attachNewNode('sun')
# Load the model for the sky
# self.sky = loader.loadModel("models/sky/solar_sky_sphere")
self.sky = loader.loadModel("models/sky/solar_sky_sphere")
# Load the texture for the sky.
self.sky_tex = loader.loadTexture("models/sky/stars_1k_tex.jpg")
# Set the sky texture to the sky model
self.sky.setTexture(self.sky_tex, 1)
# Parent the sky model to the render node so that the sky is rendered
self.sky.reparentTo(self.render)
# Scale the size of the sky.
self.sky.setScale(15000)
x = 0.005
y = 1700.0
z = 0.0
self.sky.setPos(x, y, 0)
# #milkyway 1
self.milkyWay = loader.loadModel("models/sky/planet_sphere")
self.milkWay_tex = loader.loadTexture("models/sky/milkyway_tex.jpg")
self.milkyWay.setTexture(self.milkWay_tex, 1)
self.milkyWay.reparentTo(self.milkyWayNp)
self.milkyWay.setScale(200)
self.milkyWay.setPos(x + 2000, y + 10000, z - 500)
# milkyway 2
self.milkyWay_2 = loader.loadModel("models/sky/planet_sphere")
self.milkWay_2_tex = loader.loadTexture("models/sky/milkyway_2_tex.jpg")
self.milkyWay_2.setTexture(self.milkWay_2_tex, 1)
self.milkyWay_2.reparentTo(self.milkyWay_2Np)
self.milkyWay_2.setScale(400)
self.milkyWay_2.setPos(x - 3000, y + 10000, z + 500)
# sun
self.sun = loader.loadModel("models/sky/planet_sphere")
self.sun_tex = loader.loadTexture("models/sky/sun_2_tex.jpg")
self.sun.setTexture(self.sun_tex, 1)
self.sun.reparentTo(self.sunNp)
self.sun.setScale(600)
self.sun.setPos(x + 1000, y + 10000, z + 1000)
#
# Load Mars
self.mars = loader.loadModel("models/sky/planet_sphere")
self.mars_tex = loader.loadTexture("models/sky/mars_1k_tex.jpg")
self.mars.setTexture(self.mars_tex, 1)
self.mars.reparentTo(self.marsNp)
self.mars.setScale(200)
self.mars.setPos(x + 3000, y + 10000, z + 500)
def setupSound(self):
# Set up sound
mySound = base.loader.loadSfx("sounds/Farm Morning.ogg")
mySound.play()
mySound.setVolume(3.0)
mySound.setLoop(True)
footsteps = base.loader.loadSfx("sounds/Footsteps_on_Cement-Tim_Fryer.wav")
footsteps.play()
footsteps.setVolume(0.8)
footsteps.setLoop(True)
self.jumpSound = base.loader.loadSfx("sounds/Jump-SoundBible.com-1007297584.wav")
self.jumpSound.setVolume(0.2)
self.collectSound = base.loader.loadSfx("sounds/pin_dropping-Brian_Rocca-2084700791.wav")
self.gameOverSound = base.loader.loadSfx("sounds/Bike Horn-SoundBible.com-602544869.wav")
self.levelCompleteSound = base.loader.loadSfx("sounds/Ta Da-SoundBible.com-1884170640.wav")
def setupFloor(self):
size = Vec3(7.5, 3000, 1.81818)
shape = BulletBoxShape(size * 0.55)
# shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
node = BulletRigidBodyNode('Box-Floor')
node.addShape(shape)
node.setMass(0)
stairNP = self.render.attachNewNode(node)
stairNP.setPos(0, 0, 0)
stairNP.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(stairNP.node())
modelNP = loader.loadModel('models/box.egg')
modelNP.reparentTo(stairNP)
modelNP.setPos(-size.x / 2.0, -size.y / 2.0, -size.z / 2.0)
modelNP.setScale(size)
def setupCharacter(self):
# Character
h = 1.75
w = 0.4
shape = BulletCapsuleShape(w, h - 2 * w, ZUp)
self.character = BulletCharacterControllerNode(shape, 0.4, 'Player')
self.character.setGravity(35)
self.characterNP = self.render.attachNewNode(self.character)
self.characterNP.setPos(0, 10, 5)
self.characterNP.setCollideMask(BitMask32.allOn())
self.world.attachCharacter(self.character)
self.ecco = Actor('ralph-models/ralph.egg.pz', {
'run': 'ralph-models/ralph-run.egg',
'jump': 'ralph/ralph-run.egg',
'damage': 'models/lack-damage.egg'})
self.ecco.reparentTo(self.characterNP)
self.ecco.setScale(0.7048)
self.ecco.setH(180)
def setUpCamera(self):
# If the camera is too far from ecco, move it closer.
# If the camera is too close to ecco, move it farther.
camvec = self.characterNP.getPos() - self.camera.getPos()
camvec.setZ(0.0)
camdist = camvec.length()
camvec.normalize()
if camdist > 10.0:
self.camera.setPos(self.camera.getPos() + camvec * (camdist - 40))
camdist = 10.0
if camdist < 5.0:
self.camera.setPos(self.camera.getPos() - camvec * (35 - camdist))
camdist = 5.0
def processInput(self, dt):
speed = Vec3(0, 0, 0)
if inputState.isSet('esc'): sys.exit()
if inputState.isSet('w'): speed.setY(35.0)
if inputState.isSet('arrow_left'): speed.setX(-35.0)
if inputState.isSet('arrow_right'): speed.setX(35.0)
if inputState.isSet('space'):
self.jump()
self.jumpSound.play()
if inputState.isSet('arrow_up'):
self.jump()
self.jumpSound.play()
if inputState.isSet('cam-left'): self.camera.setX(self.camera, -20 * dt)
if inputState.isSet('cam-right'): self.camera.setX(self.camera, +20 * dt)
if inputState.isSet('cam-forward'): self.camera.setY(self.camera, -200 * dt)
if inputState.isSet('cam-backward'): self.camera.setY(self.camera, +200 * dt)
# Make Ecco run
if self.isMoving is False:
self.ecco.loop("run")
self.isMoving = True
if self.pos.getY() > 1450.0:
speed.setY(0.0)
else:
speed.setY(40.0)
self.character.setLinearMovement(speed, True)
def jump(self):
self.character.setMaxJumpHeight(3.0)
self.character.setJumpSpeed(25.0)
self.character.doJump()
def setupCoins(self):
# display coins = 0
textN = TextNode('coin-score')
textN.setText(str("Coins: " + str(self.coinsCollected)))
textN.setSlant(0.1)
textNodePath = self.aspect2d.attachNewNode(textN)
textNodePath.setPos(0, 0.95, 0.9)
textNodePath.setScale(0.08)
randNum = random.sample(range(0, 1500, 200), 6)
# coins
for i in range(6):
randX = random.uniform(-3.0, 3.2)
randY = float(randNum[i])
shape = BulletSphereShape(0.3)
coinNode = BulletGhostNode('Coin-' + str(i))
coinNode.addShape(shape)
np = self.render.attachNewNode(coinNode)
np.setCollideMask(BitMask32.allOff())
np.setPos(randX, randY, 2)
# Adding sphere model
sphereNp = loader.loadModel('models/smiley.egg')
sphereNp_tex = loader.loadTexture("models/sky/coin_2_tex.jpg")
sphereNp.setTexture(sphereNp_tex, 1)
sphereNp.reparentTo(np)
sphereNp.setScale(0.45)
sphereNp.hprInterval(2.5, Vec3(360, 0, 0)).loop()
self.world.attachGhost(coinNode)
self.coins.append(coinNode)
print "node name:" + str(coinNode.getName())
def processContacts(self):
for coin in self.coins:
self.testWithSingleBody(coin)
self.coinsCollected = len(self.dictOfCoins)
def testWithSingleBody(self, secondNode):
contactResult = self.world.contactTestPair(self.character, secondNode)
if contactResult.getNumContacts() > 0:
self.collectSound.play()
for contact in contactResult.getContacts():
cp = contact.getManifoldPoint()
node0 = contact.getNode0()
node1 = contact.getNode1()
self.dictOfCoins[node1.getName()] = 1
np = self.render.find(node1.getName())
np.node().removeAllChildren()
self.world.removeGhost(np.node())
def setupObstacles(self):
# Obstacle
origin = Point3(2, 0, 0)
size = Vec3(2, 2.75, 1.5)
shape = BulletBoxShape(size * 0.55)
randNum1 = random.sample(range(0, 1500, 300), 3)
randNum2 = random.sample(range(0, 1500, 500), 3)
for i in range(2):
randX = random.uniform(-3.5, 3.5)
randY = float(randNum1[i])
pos = origin + size * i
ObstacleNP = self.render.attachNewNode(BulletRigidBodyNode('Obstacle%i' % i))
ObstacleNP.node().addShape(shape)
ObstacleNP.node().setMass(1.0)
ObstacleNP.setPos(randX, randY, 3)
ObstacleNP.setCollideMask(BitMask32.allOn())
modelNP = loader.loadModel('models/box.egg')
modelNP_tex = loader.loadTexture("models/sky/milkyway_tex.jpg")
modelNP.setTexture(modelNP_tex, 1)
modelNP.reparentTo(ObstacleNP)
# modelNP.setPos(0, 0, 0)
modelNP.setPos(-size.x / 2.0, -size.y / 2.0, -size.z / 2.0)
modelNP.setScale(size)
self.world.attachRigidBody(ObstacleNP.node())
size_2 = Vec3(3, 2.75, 1.5)
shape2 = BulletBoxShape(size_2 * 0.55)
for i in range(2):
randX = random.uniform(-3.5, 3.5)
randY = float(randNum2[i])
pos = origin + size_2 * i
pos.setY(0)
ObstacleNP = self.render.attachNewNode(BulletRigidBodyNode('ObstacleSmall%i' % i))
ObstacleNP.node().addShape(shape2)
ObstacleNP.node().setMass(1.0)
ObstacleNP.setPos(randX, randY, 2)
ObstacleNP.setCollideMask(BitMask32.allOn())
modelNP = loader.loadModel('models/box.egg')
modelNP_tex = loader.loadTexture("models/sky/moon_1k_tex.jpg")
modelNP.setTexture(modelNP_tex, 1)
modelNP.reparentTo(ObstacleNP)
# modelNP.setPos(0, 0, 0)
modelNP.setPos(-size_2.x / 2.0, -size_2.y / 2.0, -size_2.z / 2.0)
modelNP.setScale(size_2)
self.world.attachRigidBody(ObstacleNP.node())
def checkIfEccoDied(self):
print "position" + str(self.pos.getY())
if self.pos.getZ() > -50.0 and self.pos.getZ() < 0.0:
title = "Game Over"
levelCompleteN = TextNode('ecco-died')
font = loader.loadFont("font/Caveman.ttf")
levelCompleteN.setFont(font)
levelCompleteN.setText(title)
levelCompleteN.setTextColor(1, 1, 1, 1)
levelCompleteN.setSlant(0.1)
levelCompleteN.setShadow(0.03)
levelCompleteN.setShadowColor(0, 0, 200, 1)
# levelN.setFrameAsMargin(0, 0, 0, 0)
levelCompleteN.setFrameColor(200, 0, 0, 1)
levelCompleteN.setFrameLineWidth(5.0)
# textNp.node().setGlyphShift(1.0)
textNodePath = self.aspect2d.attachNewNode(levelCompleteN)
textNodePath.setPos(-0.9, 1.5, 0.5)
textNodePath.setScale(0.2)
if self.pos.getZ() < -49.0:
self.gameOverSound.play()
elif self.pos.getZ() < -50.0:
if self.gameOverSound.status() != self.gameOverSound.PLAYING:
sys.exit(1)
elif self.pos.getY() > 1300.0:
title = "Level 1 \n Complete"
levelCompleteN = TextNode('level-complete')
font = loader.loadFont("font/Caveman.ttf")
levelCompleteN.setFont(font)
levelCompleteN.setText(title)
levelCompleteN.setTextColor(1, 1, 1, 1)
levelCompleteN.setSlant(0.1)
levelCompleteN.setShadow(0.03)
levelCompleteN.setShadowColor(0, 0, 200, 1)
# levelN.setFrameAsMargin(0, 0, 0, 0)
levelCompleteN.setFrameColor(200, 0, 0, 1)
levelCompleteN.setFrameLineWidth(5.0)
# textNp.node().setGlyphShift(1.0)
textNodePath = self.aspect2d.attachNewNode(levelCompleteN)
textNodePath.setPos(-0.6, 1.5, 0.5)
textNodePath.setScale(0.2)
if self.levelCompleteSound.status() != self.levelCompleteSound.PLAYING:
self.levelCompleteSound.play()
else:
pass
def coinScoreDisplay(self):
textNp = self.aspect2d.find('coin-score')
textNp.node().clearText()
textNp.node().setText(str("Coins: " + str(self.coinsCollected)))
#Level 2
def level2(self):
titleNp2 = self.aspect2d.find('game-title')
titleNp2.removeNode()
self.level1Button.destroy()
self.level2Button.destroy()
self.sizescale2 = 0.6
self.setupWorld2()
self.setupSky2()
self.setupFloor2()
self.setupCharacter2()
# self.title = addTitle(" ")
self.inst12 = addInstructions(0.95, "[ESC]: Quit")
self.inst22 = addInstructions(0.90, "[Left key]: Turn Ecco Left")
self.inst32 = addInstructions(0.85, "[Right key]: Turn Ecco Right")
self.inst42 = addInstructions(0.80, "[Up key]: Jump Ecco")
inputState.watchWithModifiers('esc', 'escape')
inputState.watchWithModifiers('w', 'w')
inputState.watchWithModifiers('arrow_left', 'arrow_left')
inputState.watchWithModifiers('arrow_right', 'arrow_right')
inputState.watchWithModifiers('pause', 'p')
inputState.watchWithModifiers('space', 'space')
inputState.watchWithModifiers('arrow_up', 'arrow_up')
inputState.watchWithModifiers('cam-left', 'z')
inputState.watchWithModifiers('cam-right', 'x')
inputState.watchWithModifiers('cam-forward', 'c')
inputState.watchWithModifiers('cam-backward', 'v')
taskMgr.add(self.update2, "update")
# Game state variables
self.isMoving2 = False
# display framerate
self.setFrameRateMeter(True)
# Set up the camera
self.disableMouse()
self.camera.setPos(self.characterNP2.getX(), self.characterNP2.getY() - 30, 4)
self.setupSound2()
# coins variables
self.coinsCollected2 = 0
self.dictOfCoins2 = {}
self.coins2 = []
# Set up Coins as Collectables
self.setupCoins2()
# Set up Floaters with coins
self.setupFloaters2()
# Set up Obstacles
self.setupObstacles2()
# Setup Level Display
self.setupLevelDisplay2()
self.counter2 = 0
def setupLevelDisplay2(self):
LEVEL_2 = "Level 2"
levelDisplay(LEVEL_2)
levelN = TextNode('level-display')
levelN.setText(LEVEL_2)
# www.webpagepublicity.com
font = loader.loadFont("font/Caveman.ttf")
levelN.setFont(font)
levelN.setTextColor(1, 1, 1, 1)
levelN.setSlant(0.1)
levelN.setShadow(0.05)
levelN.setShadowColor(255, 0, 0, 1)
# levelN.setFrameAsMargin(0, 0, 0, 0)
# levelN.setFrameColor(0, 0, 255, 1)
# levelN.setFrameLineWidth(5.0)
# # textNp.node().setGlyphShift(1.0)
textNodePath = self.aspect2d.attachNewNode(levelN)
textNodePath.setPos(-0.45, 0, 0)
textNodePath.setScale(0.2)
def update2(self, task):
dt = globalClock.getDt()
self.pos2 = self.characterNP2.getPos()
self.counter2 = self.counter2 + 1
if self.counter2 == 150:
levelNp = self.aspect2d.find('level-display')
levelNp.removeNode()
self.setUpCamera2()
self.processInput2(dt)
self.processContacts2()
self.coinScoreDisplay2()
self.checkIfEccoDied2()
self.world2.doPhysics(dt, 10, 1 / 230.0)
return task.cont
def setupWorld2(self):
# create bullet world
self.debugNP = self.render.attachNewNode(BulletDebugNode('Debug'))
#self.debugNP.show()
self.world2 = BulletWorld()
self.world2.setGravity(Vec3(0, 0, -9.81))
self.world2.setDebugNode(self.debugNP.node())
def setupSky2(self):
self.milkyWayNp = render.attachNewNode('milkyway')
self.milkyWay_2Np = render.attachNewNode('milkyway_2')
self.marsNp = render.attachNewNode('mars')
self.sunNp = render.attachNewNode('sun')
# Load the model for the sky
# self.sky = loader.loadModel("models/sky/solar_sky_sphere")
self.sky = loader.loadModel("models/sky/solar_sky_sphere")
# Load the texture for the sky.
self.sky_tex = loader.loadTexture("models/sky/stars_1k_tex.jpg")
# Set the sky texture to the sky model
self.sky.setTexture(self.sky_tex, 1)
# Parent the sky model to the render node so that the sky is rendered
self.sky.reparentTo(self.render)
# Scale the size of the sky.
self.sky.setScale(15000)
x = 0.005
y = 1700.0
z = 0.0
self.sky.setPos(x, y, 0)
# #milkyway 1
self.milkyWay = loader.loadModel("models/sky/planet_sphere")
self.milkWay_tex = loader.loadTexture("models/sky/milkyway_tex.jpg")
self.milkyWay.setTexture(self.milkWay_tex, 1)
self.milkyWay.reparentTo(self.milkyWayNp)
self.milkyWay.setScale(200)
self.milkyWay.setPos(x + 2000, y + 10000, z - 500)
# milkyway 2
self.milkyWay_2 = loader.loadModel("models/sky/planet_sphere")
self.milkWay_2_tex = loader.loadTexture("models/sky/milkyway_2_tex.jpg")
self.milkyWay_2.setTexture(self.milkWay_2_tex, 1)
self.milkyWay_2.reparentTo(self.milkyWay_2Np)
self.milkyWay_2.setScale(400)
self.milkyWay_2.setPos(x - 3000, y + 10000, z + 500)
# sun
self.sun = loader.loadModel("models/sky/planet_sphere")
self.sun_tex = loader.loadTexture("models/sky/sun_2_tex.jpg")
self.sun.setTexture(self.sun_tex, 1)
self.sun.reparentTo(self.sunNp)
self.sun.setScale(600)
self.sun.setPos(x + 1000, y + 10000, z + 1000)
#
# Load Mars
self.mars = loader.loadModel("models/sky/planet_sphere")
self.mars_tex = loader.loadTexture("models/sky/mars_1k_tex.jpg")
self.mars.setTexture(self.mars_tex, 1)
self.mars.reparentTo(self.marsNp)
self.mars.setScale(200)
self.mars.setPos(x + 3000, y + 10000, z + 500)
def setUpCamera2(self):
# If the camera is too far from ecco, move it closer.
# If the camera is too close to ecco, move it farther.
camvec = self.characterNP2.getPos() - self.camera.getPos()
camvec.setZ(0.0)
camdist = camvec.length()
camvec.normalize()
if camdist > 10.0:
self.camera.setPos(self.camera.getPos() + camvec * (camdist - 40))
camdist = 10.0
if camdist < 5.0:
self.camera.setPos(self.camera.getPos() - camvec * (35 - camdist))
camdist = 5.0
def setupSound2(self):
# Set up sound
mySound = base.loader.loadSfx("sounds/Farm Morning.ogg")
mySound.play()
mySound.setVolume(3.0)
mySound.setLoop(True)
footsteps = base.loader.loadSfx("sounds/Footsteps_on_Cement-Tim_Fryer.wav")
footsteps.play()
footsteps.setVolume(0.8)
footsteps.setLoop(True)
self.jumpSound2 = base.loader.loadSfx("sounds/Jump-SoundBible.com-1007297584.wav")
self.jumpSound2.setVolume(0.2)
self.collectSound2 = base.loader.loadSfx("sounds/pin_dropping-Brian_Rocca-2084700791.wav")
self.gameOverSound2 = base.loader.loadSfx("sounds/Bike Horn-SoundBible.com-602544869.wav")
self.levelCompleteSound2 = base.loader.loadSfx("sounds/Ta Da-SoundBible.com-1884170640.wav")
def setupFloor2(self):
size = Vec3(7.5, 3000, 1.81818)
shape = BulletBoxShape(size * 0.55)
# shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
node = BulletRigidBodyNode('Box-Floor')
node.addShape(shape)
node.setMass(0)
stairNP = self.render.attachNewNode(node)
stairNP.setPos(0, 0, 0)
stairNP.setCollideMask(BitMask32.allOn())
self.world2.attachRigidBody(stairNP.node())
modelNP = loader.loadModel('models/box.egg')
modelNP.reparentTo(stairNP)
modelNP.setPos(-size.x / 2.0, -size.y / 2.0, -size.z / 2.0)
modelNP.setScale(size)
def setupCharacter2(self):
# Character
h = 1.75
w = 0.4
shape = BulletCapsuleShape(w, h - 2 * w, ZUp)
self.character2 = BulletCharacterControllerNode(shape, 0.4, 'Player')
self.character2.setGravity(35)
self.characterNP2 = self.render.attachNewNode(self.character2)
self.characterNP2.setPos(0, 10, 5)
self.characterNP2.setCollideMask(BitMask32.allOn())
self.world2.attachCharacter(self.character2)
self.ecco2 = Actor('ralph-models/ralph.egg.pz', {
'run': 'ralph-models/ralph-run.egg',
'jump': 'ralph/ralph-run.egg',
'damage': 'models/lack-damage.egg'})
self.ecco2.reparentTo(self.characterNP2)
self.ecco2.setScale(0.7048)
self.ecco2.setH(180)
def processInput2(self, dt):
speed = Vec3(0, 0, 0)
if inputState.isSet('esc'): sys.exit()
if inputState.isSet('w'): speed.setY(35.0)
if inputState.isSet('arrow_left'): speed.setX(-35.0)
if inputState.isSet('arrow_right'): speed.setX(35.0)
if inputState.isSet('space'):
self.jump2()
self.jumpSound2.play()
if inputState.isSet('arrow_up'):
self.jump2()
self.jumpSound2.play()
if inputState.isSet('cam-left'): self.camera.setX(self.camera, -20 * dt)
if inputState.isSet('cam-right'): self.camera.setX(self.camera, +20 * dt)
if inputState.isSet('cam-forward'): self.camera.setY(self.camera, -200 * dt)
if inputState.isSet('cam-backward'): self.camera.setY(self.camera, +200 * dt)
# Make Ecco run
if self.isMoving2 is False:
self.ecco2.loop("run")
self.isMoving2 = True
if self.pos2.getY() > 1450.0:
speed.setY(0.0)
else:
speed.setY(40.0)
self.character2.setLinearMovement(speed, True)
def jump2(self):
self.character2.setMaxJumpHeight(3.0)
self.character2.setJumpSpeed(25.0)
self.character2.doJump()
def setupCoins2(self):
# display coins = 0
textN = TextNode('coin-score')
textN.setText(str("Coins: " + str(self.coinsCollected2)))
textN.setSlant(0.1)
textNodePath = self.aspect2d.attachNewNode(textN)
textNodePath.setPos(0, 0.95, 0.9)
textNodePath.setScale(0.08)
randNum = random.sample(range(0, 1500, 200), 6)
# coins
for i in range(6):
randX = random.uniform(-3.0, 3.2)
randY = float(randNum[i])
shape = BulletSphereShape(0.3)
coinNode = BulletGhostNode('Coin-' + str(i))
coinNode.addShape(shape)
np = self.render.attachNewNode(coinNode)
np.setCollideMask(BitMask32.allOff())
np.setPos(randX, randY, 2)
# Adding sphere model
sphereNp = loader.loadModel('models/smiley.egg')
sphereNp_tex = loader.loadTexture("models/sky/coin_2_tex.jpg")
sphereNp.setTexture(sphereNp_tex, 1)
sphereNp.reparentTo(np)
sphereNp.setScale(0.45)
sphereNp.hprInterval(2.5, Vec3(360, 0, 0)).loop()
self.world2.attachGhost(coinNode)
self.coins2.append(coinNode)
print "node name:" + str(coinNode.getName())
def setupObstacles2(self):
# Obstacle
origin = Point3(2, 0, 0)
size = Vec3(2, 2.75, 1.5)
shape = BulletBoxShape(size * 0.55)
randNum1 = random.sample(range(0, 1500, 300), 3)
randNum2 = random.sample(range(0, 1500, 500), 3)
for i in range(2):
randX = random.uniform(-3.5, 3.5)
randY = float(randNum1[i])
pos = origin + size * i
ObstacleNP = self.render.attachNewNode(BulletRigidBodyNode('Obstacle%i' % i))
ObstacleNP.node().addShape(shape)
ObstacleNP.node().setMass(1.0)
ObstacleNP.setPos(randX, randY, 3)
ObstacleNP.setCollideMask(BitMask32.allOn())
modelNP = loader.loadModel('models/box.egg')
modelNP_tex = loader.loadTexture("models/sky/milkyway_tex.jpg")
modelNP.setTexture(modelNP_tex, 1)
modelNP.reparentTo(ObstacleNP)
# modelNP.setPos(0, 0, 0)
modelNP.setPos(-size.x / 2.0, -size.y / 2.0, -size.z / 2.0)
modelNP.setScale(size)
self.world2.attachRigidBody(ObstacleNP.node())
size_2 = Vec3(3, 2.75, 1.5)
shape2 = BulletBoxShape(size_2 * 0.55)
for i in range(2):
randX = random.uniform(-3.5, 3.5)
randY = float(randNum2[i])
pos = origin + size_2 * i
pos.setY(0)
ObstacleNP = self.render.attachNewNode(BulletRigidBodyNode('ObstacleSmall%i' % i))
ObstacleNP.node().addShape(shape2)
ObstacleNP.node().setMass(1.0)
ObstacleNP.setPos(randX, randY, 2)
ObstacleNP.setCollideMask(BitMask32.allOn())
modelNP = loader.loadModel('models/box.egg')
modelNP_tex = loader.loadTexture("models/sky/moon_1k_tex.jpg")
modelNP.setTexture(modelNP_tex, 1)
modelNP.reparentTo(ObstacleNP)
# modelNP.setPos(0, 0, 0)
modelNP.setPos(-size_2.x / 2.0, -size_2.y / 2.0, -size_2.z / 2.0)
modelNP.setScale(size_2)
self.world2.attachRigidBody(ObstacleNP.node())
def setupFloaters2(self):
size = Vec3(3.5, 5.5, 0.3)
randNum = random.sample(range(10, 1500, 500), 3)
for i in range(3):
randX = random.randrange(-2, 3, 10)
randY = float(randNum[i])
# randY = random.randint(1000, 1500)
shape = BulletBoxShape(size * 0.55)
node = BulletRigidBodyNode('Floater')
node.setMass(0)
node.addShape(shape)
np = self.render.attachNewNode(node)
# np.setPos(9, 30, 3)
np.setPos(randX, randY, 6)
np.setR(0)
self.world2.attachRigidBody(node)
dummyNp = self.render.attachNewNode('milkyway')
dummyNp.setPos(randX, randY, 6)
modelNP = loader.loadModel('models/box.egg')
modelNP_tex = loader.loadTexture("models/sky/moon_tex.jpg")
modelNP.setTexture(modelNP_tex, 1)
modelNP.reparentTo(dummyNp)
modelNP.setPos(-1, 0, -1)
modelNP.setPos(-size.x / 2.0, -size.y / 2.0, -size.z / 2.0)
modelNP.setScale(size)
dummyNp.hprInterval(2.5, Vec3(360, 0, 0)).loop()
# Put A Coin On the Floater
shape = BulletSphereShape(0.75)
coinNode = BulletGhostNode('FloaterCoin-' + str(i))
coinNode.addShape(shape)
np = self.render.attachNewNode(coinNode)
np.setCollideMask(BitMask32.allOff())
# np.setPos(randX, randY, 2)
np.setPos(randX, randY, 7.0)
# Adding sphere model
sphereNp = loader.loadModel('models/smiley.egg')
sphereNp_tex = loader.loadTexture("models/sky/coin_2_tex.jpg")
sphereNp.setTexture(sphereNp_tex, 1)
sphereNp.reparentTo(np)
sphereNp.setScale(0.85)
sphereNp.hprInterval(1.5, Vec3(360, 0, 0)).loop()
self.world2.attachGhost(coinNode)
self.coins2.append(coinNode)
print "node name:" + str(coinNode.getName())
def processContacts2(self):
for coin in self.coins2:
self.testWithSingleBody2(coin)
self.coinsCollected2 = len(self.dictOfCoins2)
def testWithSingleBody2(self, secondNode):
contactResult = self.world2.contactTestPair(self.character2, secondNode)
if contactResult.getNumContacts() > 0:
self.collectSound2.play()
for contact in contactResult.getContacts():
cp = contact.getManifoldPoint()
node0 = contact.getNode0()
node1 = contact.getNode1()
self.dictOfCoins2[node1.getName()] = 1
np = self.render.find(node1.getName())
np.node().removeAllChildren()
self.world2.removeGhost(np.node())
def checkIfEccoDied2(self):
print "position" + str(self.pos2.getY())
if self.pos2.getZ() > -50.0 and self.pos2.getZ() < 0.0:
title = "Game Over"
levelCompleteN = TextNode('ecco-died')
font = loader.loadFont("font/Caveman.ttf")
levelCompleteN.setFont(font)
levelCompleteN.setText(title)
levelCompleteN.setTextColor(1, 1, 1, 1)
levelCompleteN.setSlant(0.1)
levelCompleteN.setShadow(0.03)
levelCompleteN.setShadowColor(0, 0, 200, 1)
# levelN.setFrameAsMargin(0, 0, 0, 0)
levelCompleteN.setFrameColor(200, 0, 0, 1)
levelCompleteN.setFrameLineWidth(5.0)
# textNp.node().setGlyphShift(1.0)
textNodePath = self.aspect2d.attachNewNode(levelCompleteN)
textNodePath.setPos(-0.9, 1.5, 0.5)
textNodePath.setScale(0.2)
if self.pos2.getZ() < -49.0:
self.gameOverSound2.play()
elif self.pos2.getZ() < -50.0:
if self.gameOverSound2.status() != self.gameOverSound2.PLAYING:
sys.exit(1)
elif self.pos2.getY() > 1300.0:
title = "Level 2 \n Complete"
levelCompleteN = TextNode('level-complete')
font = loader.loadFont("font/Caveman.ttf")
levelCompleteN.setFont(font)
levelCompleteN.setText(title)
levelCompleteN.setTextColor(1, 1, 1, 1)
levelCompleteN.setSlant(0.1)
levelCompleteN.setShadow(0.03)
levelCompleteN.setShadowColor(0, 0, 200, 1)
# levelN.setFrameAsMargin(0, 0, 0, 0)
levelCompleteN.setFrameColor(200, 0, 0, 1)
levelCompleteN.setFrameLineWidth(5.0)
# textNp.node().setGlyphShift(1.0)
textNodePath = self.aspect2d.attachNewNode(levelCompleteN)
textNodePath.setPos(-0.6, 1.5, 0.5)
textNodePath.setScale(0.2)
if self.levelCompleteSound2.status() != self.levelCompleteSound2.PLAYING:
self.levelCompleteSound2.play()
else:
pass
def coinScoreDisplay2(self):
textNp = self.aspect2d.find('coin-score')
textNp.node().clearText()
textNp.node().setText(str("Coins: " + str(self.coinsCollected2)))
class GameStatePlaying(GState):
VIDAS = 3
#LEVEL_TIME = 100
LEVEL_TIME = 50
def start(self):
self._playing_node = render.attachNewNode("playing-node")
self._playing_node2d = aspect2d.attachNewNode("playing2d-node")
self.keyMap = {"left": 0, "right": 0, "up": 0, "down": 0}
base.accept("escape", sys.exit)
props = WindowProperties()
props.setCursorHidden(True)
base.disableMouse()
props.setMouseMode(WindowProperties.MRelative)
base.win.requestProperties(props)
base.win.movePointer(0,
base.win.getXSize() / 2,
base.win.getYSize() / 2)
self._modulos = None
self._paneles = None
self._num_lvl = 1
self._num_lvls = 2
self.loadLevel()
self.loadGUI()
self.loadBkg()
self._last_t = None
self._last_t_space = 0
def stop(self):
self._playing_node.removeNode()
self._playing_node2d.removeNode()
def loadLevel(self):
self._level_time = self.LEVEL_TIME
self.initBullet()
self._player = Player(self.world)
self.loadMap()
self.setAI()
def initBullet(self):
self.world = BulletWorld()
#self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setGravity(Vec3(0, 0, -1.62))
groundShape = BulletPlaneShape(Vec3(0, 0, 1), 0)
groundNode = BulletRigidBodyNode('Ground')
groundNode.addShape(groundShape)
self.world.attachRigidBody(groundNode)
def loadBkg(self):
self.environ1 = loader.loadModel("../data/models/skydome")
self.environ1.reparentTo(self._playing_node)
self.environ1.setPos(0, 0, 0)
self.environ1.setScale(1)
self.environ2 = loader.loadModel("../data/models/skydome")
self.environ2.reparentTo(self._playing_node)
self.environ2.setP(180)
self.environ2.setH(270)
self.environ2.setScale(1)
self.dirnlight1 = DirectionalLight("dirn_light1")
self.dirnlight1.setColor(Vec4(1.0, 1.0, 1.0, 1.0))
self.dirnlightnode1 = self._playing_node.attachNewNode(self.dirnlight1)
self.dirnlightnode1.setHpr(0, 317, 0)
self._playing_node.setLight(self.dirnlightnode1)
self.alight = AmbientLight('alight')
self.alight.setColor(VBase4(0.05, 0.05, 0.05, 1))
self.alight_node = self._playing_node.attachNewNode(self.alight)
self._playing_node.setLight(self.alight_node)
self.environ1 = loader.loadModel("../data/models/ground")
self.environ1.reparentTo(self._playing_node)
self.environ1.setPos(0, 0, 0)
self.environ1.setScale(1)
def loadGUI(self):
self.vidas_imgs = list()
w = 0.24
for i in range(self.VIDAS):
image_warning = OnscreenImage(
image='../data/Texture/signal_warning.png',
pos=(-1 + i * w, 0, 0.85),
parent=self._playing_node2d)
image_warning.setScale(0.1)
image_warning.setTransparency(TransparencyAttrib.MAlpha)
image_warning.hide()
image_ok = OnscreenImage(image='../data/Texture/signal_ok.png',
pos=(-1 + i * w, 0, 0.85),
parent=self._playing_node2d)
image_ok.setScale(0.1)
image_ok.setTransparency(TransparencyAttrib.MAlpha)
image_ok.show()
self.vidas_imgs.append((image_ok, image_warning))
self._level_time_O = OnscreenText(text='',
pos=(0, 0.85),
scale=0.14,
fg=(1.0, 1.0, 1.0, 1.0),
bg=(0.0, 0.0, 0.0, 1.0),
parent=self._playing_node2d)
def loadMap(self):
if (self._modulos is not None):
for m in self._modulos:
m.remove()
if (self._paneles is not None):
for p in self._paneles:
p.remove()
self._tp = TiledParser("map" + str(self._num_lvl))
self._modulos, self._paneles = self._tp.load_models(
self.world, self._playing_node)
def setAI(self):
taskMgr.add(self.update, 'Update')
def update(self, task):
if (task.frame > 1):
self.world.doPhysics(globalClock.getDt())
self._level_time -= globalClock.getDt()
self._level_time_O.setText(str(int(self._level_time)))
for panel in self._paneles:
contact = self.world.contactTestPair(self._player.getRBNode(),
panel.getRBNode())
if contact.getNumContacts() > 0:
panel.manipulate()
brokens = 0
for panel in self._paneles:
if panel.isBroken():
brokens += 1
#print brokens
for i, vida_imgs in enumerate(self.vidas_imgs):
if i < len(self.vidas_imgs) - brokens:
vida_imgs[0].show()
vida_imgs[1].hide()
else:
vida_imgs[0].hide()
vida_imgs[1].show()
if brokens > self.VIDAS:
self.gameOver()
return task.done
if self._level_time <= 0:
self._num_lvl += 1
if self._num_lvl <= self._num_lvls:
self.nextLevel()
else:
self.theEnd()
return task.done
return task.cont
def gameOver(self):
taskMgr.remove('player-task')
taskMgr.remove('panel-task')
taskMgr.remove('panel-sound-task')
self._mission_text_O = OnscreenText(text='Game Over',
pos=(0, 0),
scale=0.5,
fg=(1.0, 1.0, 1.0, 1.0),
bg=(0.0, 0.0, 0.0, 1.0))
taskMgr.add(self.gameOverTransition, 'game-over-transition')
#self.loadLevel()
print "Game Over"
def gameOverTransition(self, task):
base.win.movePointer(0,
base.win.getXSize() / 2,
base.win.getYSize() / 2)
if task.time > 3.0:
self._mission_text_O.hide()
props = WindowProperties()
props.setCursorHidden(False)
base.win.requestProperties(props)
self._state_context.changeState(
gameStateMenu.GameStateMenu(self._state_context))
print "MENU"
return task.done
return task.cont
def nextLevel(self):
taskMgr.remove('player-task')
taskMgr.remove('panel-task')
taskMgr.remove('panel-sound-task')
self._mission_text_O = OnscreenText(text='Mission\nComplete',
pos=(0, 0),
scale=0.5,
fg=(1.0, 1.0, 1.0, 1.0),
bg=(0.0, 0.0, 0.0, 1.0))
taskMgr.add(self.nextLevelTransition, 'next-Level-transition')
print "Mission Complete"
def nextLevelTransition(self, task):
base.win.movePointer(0,
base.win.getXSize() / 2,
base.win.getYSize() / 2)
if task.time > 3.0:
print "Next Level"
self._mission_text_O.hide()
self.loadLevel()
return task.done
return task.cont
def theEnd(self):
taskMgr.remove('player-task')
taskMgr.remove('panel-task')
taskMgr.remove('panel-sound-task')
self._mission_text_O = OnscreenText(text='. The End .',
pos=(0, 0),
scale=0.5,
fg=(1.0, 1.0, 1.0, 1.0),
bg=(0.0, 0.0, 0.0, 1.0))
taskMgr.add(self.theEndTransition, 'theEnd-transition')
#self.loadLevel()
print "Mission Complete"
def theEndTransition(self, task):
base.win.movePointer(0,
base.win.getXSize() / 2,
base.win.getYSize() / 2)
if task.time > 3.0:
self._mission_text_O.hide()
props = WindowProperties()
props.setCursorHidden(False)
base.win.requestProperties(props)
self._state_context.changeState(
gameStateMenu.GameStateMenu(self._state_context))
print "The End"
return task.done
return task.cont
class DroneEnv():
def __init__(self):
self.visualize = False
self.actors = 2
#repair this in drone class as well
if self.visualize == False :
from pandac.PandaModules import loadPrcFileData
loadPrcFileData("", "window-type none")
import direct.directbase.DirectStart
self.ep = 0
self.ep_rew = 0
self.t = 0
self.action_space = Box(-1,1,shape=((self.actors*3),))
self.observation_space = Box(-50,50,shape=((self.actors*3 + 3),))
self.target = 8*np.random.rand(3)
self.construct()
self.percentages = []
self.percentMean = []
self.percentStd = []
taskMgr.add(self.stepTask, 'update')
taskMgr.add(self.lightTask, 'lights')
self.forces = np.array([0,0,9.81]*self.actors, dtype = np.float)
def construct(self) :
if self.visualize :
base.cam.setPos(17,17,1)
base.cam.lookAt(0, 0, 0)
wp = WindowProperties()
wp.setSize(1200, 500)
base.win.requestProperties(wp)
# World
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
#skybox
skybox = loader.loadModel('../models/skybox.gltf')
skybox.setTexGen(TextureStage.getDefault(), TexGenAttrib.MWorldPosition)
skybox.setTexProjector(TextureStage.getDefault(), render, skybox)
skybox.setTexScale(TextureStage.getDefault(), 1)
skybox.setScale(100)
skybox.setHpr(0, -90, 0)
tex = loader.loadCubeMap('../textures/s_#.jpg')
skybox.setTexture(tex)
skybox.reparentTo(render)
render.setTwoSided(True)
#Light
plight = PointLight('plight')
plight.setColor((1.0, 1.0, 1.0, 1))
plnp = render.attachNewNode(plight)
plnp.setPos(0, 0, 0)
render.setLight(plnp)
# Create Ambient Light
ambientLight = AmbientLight('ambientLight')
ambientLight.setColor((0.15, 0.05, 0.05, 1))
ambientLightNP = render.attachNewNode(ambientLight)
render.setLight(ambientLightNP)
#multiple drone initialization
wrgDistance = True
positions = []
self.uavs = [Drone.uav() for i in range(self.actors)]
[self.world.attachRigidBody(uav.drone) for uav in self.uavs]
[uav.drone.setDeactivationEnabled(False) for uav in self.uavs]
#target object
self.targetObj = loader.loadModel("../models/target.gltf")
self.targetObj.reparentTo(render)
self.targetObj.setPos(Vec3(self.target[0], self.target[1], self.target[2]))
def lightTask(self, task) :
count = globalClock.getFrameCount()
for uav in self.uavs :
rest = count % 100
if rest < 10 :
uav.plight2.setColor((0.1, 0.9, 0.1, 1))
elif rest > 30 and rest < 40 :
uav.plight2.setColor((0.9, 0.1, 0.1, 1))
elif rest > 65 and rest < 70 :
uav.plight2.setColor((0.9,0.9, 0.9, 1))
elif rest > 75 and rest < 80 :
uav.plight2.setColor((0.9,0.9, 0.9, 1))
else :
uav.plight2.setColor((0.1, 0.1, 0.1, 1))
return task.cont
def getState(self) :
stateVec = []
for uav in self.uavs :
pos = uav.drone.transform.pos
stateVec.append(pos)
stateVec.append(self.target)
state = np.array(stateVec).reshape((3*self.actors + 3),)
state = np.around(state, decimals = 2)
state = np.expand_dims(state, 0)
return state
def getReward(self) :
reward1 = 0
reward2 = 0
distance = 0
pos1 = self.uavs[0].drone.transform.pos
d1 = np.linalg.norm(pos1 - self.target)
if d1 < 10 :
reward1 = 10 - d1
reward1 = reward1/50
pos2 = self.uavs[1].drone.transform.pos
d2 = np.linalg.norm(pos2 - self.target)
if d2 < 10 :
reward2 = 10 - d2
reward2 = reward2/50
return reward1, reward2
def reset(self):
#log
self.percentages.append(self.ep_rew)
me = np.mean(self.percentages[-500:])
self.percentMean.append(me)
self.percentStd.append(np.std(self.percentages[-500:]))
state = self.getState()
vel1 = self.uavs[0].drone.get_linear_velocity()
velocity1 = np.nan_to_num(np.array([vel1[0], vel1[1], vel1[2]]))
v1 = np.mean(np.abs(velocity1))
vel2 = self.uavs[1].drone.get_linear_velocity()
velocity2 = np.nan_to_num(np.array([vel2[0], vel2[1], vel2[2]]))
v2 = np.mean(np.abs(velocity2))
print(f'{self.ep} {self.t} {self.ep_rew:+8.2f} {me:+6.2f} {v1:+4.2f} {v2:+4.2f} {state}')
#prevPos = 8*(np.random.rand(3)-0.5) + 8*(np.random.rand(3)-0.5) #- np.array([0,0,2], np.float32)
#physics reset
for uav in self.uavs :
#inPos = prevPos + 8*(np.random.rand(3)-0.5) + 8*(np.random.rand(3)-0.5)
#prevPos = np.copy(inPos)
#uav.body.setPos(inPos[0], inPos[1], inPos[2])
uav.body.setHpr(0, 0, 0)
uav.drone.set_linear_velocity(Vec3(0,0,0))
uav.drone.setAngularVelocity(Vec3(0,0,0))
self.uavs[0].body.setPos(-4, 0, -4)
self.uavs[1].body.setPos(4,0,4)
self.forces = np.array([0,0,9.81]*self.actors, dtype = np.float)
#define new target:
self.target = np.zeros((3)) #8*(2*np.random.rand(3)-1)
self.target[2] = np.abs(self.target[2])
self.targetObj.setPos(Vec3(self.target[0], self.target[1], self.target[2]))
self.ep +=1
self.t = 0
self.ep_rew = 0
state = self.getState()
return state
def stepTask(self, task) :
dt = globalClock.getDt()
if self.visualize :
self.world.doPhysics(dt)
else :
self.world.doPhysics(0.1)
for i, uav in enumerate(self.uavs) :
force = self.forces[i*3: (i+1)*3]
forceVec3 = Vec3(force[0], force[1], force[2])
if np.abs(force[0]) > 0 or np.abs(force[1]) > 0 or np.abs(force[2]) > 0 :
uav.drone.applyCentralForce(forceVec3)
return task.cont
def step(self, action):
done = False
reward = 0
self.t += 1
reward1, reward2 = self.getReward()
self.ep_rew += reward1 + reward2
state = self.getState()
basis = np.array([0,0,9.81]*self.actors, dtype = np.float)
self.forces = 0.1*action + basis #0.05*action + basis
#10 sub steps in each step
for i in range(5) :
c = taskMgr.step()
self.forces -= 0.05*(self.forces - basis)
#collision test
if self.world.contactTestPair(self.uavs[0].drone, self.uavs[1].drone).getNumContacts() > 0 :
done = True
break
#time constraint
if self.t > 150 :
done = True
#position constraint :
for uav in self.uavs :
pos = uav.drone.transform.pos
if np.max(np.abs(pos)) > 30 :
done = True
return state, reward1, reward2, done, {}
