def grpshow(self, base):
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
idfreetabletopplacement = int(result[3][0])
objrotmat = dc.strToMat4(result[3][1])
# show object
geom = pg.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom)
star = NodePath('obj')
star.attachNewNode(node)
star.setColor(Vec4(.77,0.67,0,1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
sql = "SELECT freetabletopgrip.rotmat, freetabletopgrip.jawwidth FROM freetabletopgrip WHERE \
freetabletopgrip.idfreetabletopplacement=%d" % idfreetabletopplacement
result = self.gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .1])
tmprtq85.setMat(pandanpmat4 = hndrotmat)
tmprtq85.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmprtq85.reparentTo(base.render)
def grpshow(self, base):
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
idfreetabletopplacement = int(result[3][0])
objrotmat = dc.strToMat4(result[3][1])
# show object
geom = pg.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom)
star = NodePath('obj')
star.attachNewNode(node)
star.setColor(Vec4(.77, 0.67, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
sql = "SELECT freetabletopgrip.rotmat, freetabletopgrip.jawwidth FROM freetabletopgrip WHERE \
freetabletopgrip.idfreetabletopplacement=%d" % idfreetabletopplacement
result = self.gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .1])
tmprtq85.setMat(hndrotmat)
tmprtq85.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmprtq85.reparentTo(base.render)
def loadFreeTabletopPlacement(self, objname):
"""
load self.freegripid, etc. from mysqldatabase
:param handname which hand to use, rtq85 by default
:return: a list of [freegripid, freegripcontacts, freegripnormals, freegriprotmats, freegripjawwidth]
author: weiwei
date: 20170112
"""
tpsmat4s = []
# access to db
sql = "SELECT freetabletopplacement.rotmat FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % objname
data = self.execute(sql)
if len(data) != 0:
for i in range(len(data)):
tpsmat4s.append(dc.strToMat4(data[i][0]))
return tpsmat4s
else:
print "Plan tabletoplacements using freetabletopplacement.removebadfacets first!"
return None
def loadFreeAirGrip(self, objname, handname = "rtq85"):
"""
load self.freegripid, etc. from mysqldatabase
:param handname which hand to use, rtq85 by default
:return: a list of [freegripid, freegripcontacts, freegripnormals, freegriprotmats, freegripjawwidth]
author: weiwei
date: 20170112
"""
freegripid = []
freegripcontacts = []
freegripnormals = []
freegriprotmats = []
freegripjawwidth = []
# access to db
sql = "SELECT freeairgrip.idfreeairgrip, freeairgrip.contactpnt0, freeairgrip.contactpnt1, \
freeairgrip.contactnormal0, freeairgrip.contactnormal1, freeairgrip.rotmat, \
freeairgrip.jawwidth FROM freeairgrip, hand, object \
WHERE freeairgrip.idobject = object.idobject AND object.name like '%s' \
AND freeairgrip.idhand = hand.idhand AND hand.name like '%s'" % (objname, handname)
data = self.execute(sql)
if len(data) != 0:
for i in range(len(data)):
freegripid.append(int(data[i][0]))
freegripcontacts.append([dc.strToV3(data[i][1]), dc.strToV3(data[i][2])])
freegripnormals.append([dc.strToV3(data[i][3]), dc.strToV3(data[i][4])])
freegriprotmats.append(dc.strToMat4(data[i][5]))
freegripjawwidth.append(float(data[i][6]))
return [freegripid, freegripcontacts, freegripnormals, freegriprotmats, freegripjawwidth]
else:
return None
def loadFreeAirGrip(self, objname, handname = "rtq85"):
"""
load self.freegripid, etc. from mysqldatabase
:param handname which hand to use, rtq85 by default
:return: a list of [freegripid, freegripcontacts, freegripnormals, freegriprotmats, freegripjawwidth]
author: weiwei
date: 20170112
"""
freegripid = []
freegripcontacts = []
freegripnormals = []
freegriprotmats = []
freegripjawwidth = []
# access to db
sql = "SELECT freeairgrip.idfreeairgrip, freeairgrip.contactpnt0, freeairgrip.contactpnt1, \
freeairgrip.contactnormal0, freeairgrip.contactnormal1, freeairgrip.rotmat, \
freeairgrip.jawwidth FROM freeairgrip, hand, object \
WHERE freeairgrip.idobject = object.idobject AND object.name like '%s' \
AND freeairgrip.idhand = hand.idhand AND hand.name like '%s'" % (objname, handname)
data = self.execute(sql)
if len(data) != 0:
for i in range(len(data)):
freegripid.append(int(data[i][0]))
freegripcontacts.append([dc.strToV3(data[i][1]), dc.strToV3(data[i][2])])
freegripnormals.append([dc.strToV3(data[i][3]), dc.strToV3(data[i][4])])
freegriprotmats.append(dc.strToMat4(data[i][5]))
freegripjawwidth.append(float(data[i][6]))
return [freegripid, freegripcontacts, freegripnormals, freegriprotmats, freegripjawwidth]
else:
return None
def loadFreeTabletopPlacement(self, objname):
"""
load self.freegripid, etc. from mysqldatabase
:param handname which hand to use, rtq85 by default
:return: a list of [freegripid, freegripcontacts, freegripnormals, freegriprotmats, freegripjawwidth]
author: weiwei
date: 20170112
"""
tpsmat4s = []
# access to db
sql = "SELECT freetabletopplacement.rotmat FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % objname
data = self.execute(sql)
if len(data) != 0:
for i in range(len(data)):
tpsmat4s.append(dc.strToMat4(data[i][0]))
return tpsmat4s
else:
print "Plan tabletoplacements using freetabletopplacement.removebadfacets first!"
return None
def __loadDropFreeGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: jiayao
date: 20170821
"""
# freeairgripdata = self.gdb.loadDropFreeGrip(self.dbobjname, self.handname)
# if freeairgripdata is None:
# raise ValueError("Plan the DropFreeGrip first!")
#
# self.freegripid = freeairgripdata[0]
# self.freegripcontacts = freeairgripdata[1]
# self.freegripnormals = freeairgripdata[2]
# self.freegriprotmats = freeairgripdata[3]
# self.freegripjawwidth = freeairgripdata[4]
handname = "rtq85"
freegripid = []
freegripcontacts = []
freegripnormals = []
freegriprotmats = []
freegripjawwidth = []
# access to db
gdb = db.GraspDB()
sql = "SELECT dropfreegrip.iddropfreegrip, dropfreegrip.contactpnt0, dropfreegrip.contactpnt1, \
dropfreegrip.contactnormal0, dropfreegrip.contactnormal1, dropfreegrip.rotmat, \
dropfreegrip.jawwidth FROM dropfreegrip, hand, object\
WHERE dropfreegrip.idobject = object.idobject AND object.name like '%s' \
AND dropfreegrip.idhand = hand.idhand AND hand.name like '%s' \
" % (self.dbobjname, handname)
data = gdb.execute(sql)
if len(data) != 0:
for i in range(len(data)):
freegripid.append(int(data[i][0]))
freegripcontacts.append([dc.strToV3(data[i][1]), dc.strToV3(data[i][2])])
freegripnormals.append([dc.strToV3(data[i][3]), dc.strToV3(data[i][4])])
freegriprotmats.append(dc.strToMat4(data[i][5]))
freegripjawwidth.append(float(data[i][6]))
else:
print "no DropFreeGrip select"
self.freegripid = freegripid
self.freegripcontacts = freegripcontacts
self.freegripnormals = freegripnormals
self.freegriprotmats = freegriprotmats
self.freegripjawwidth = freegripjawwidth
print "ok"
def grpshow(self, base, gdb):
sql = "SELECT tabletopplacements.idtabletopplacements, tabletopplacements.rotmat \
FROM tabletopplacements,freetabletopplacement,object,angle WHERE \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s' AND \
tabletopplacements.idangle = angle.idangle AND \
freetabletopplacement.idfreetabletopplacement = %d AND angle.value = %d" % (
self.dbobjname, 1, 45)
# WHY 1 and why 45.0?
result = gdb.execute(sql)
print("SHOW RESULT LEN = " + str(len(result)))
if len(result) != 0:
for resultrow in result:
idtabletopplacements = int(resultrow[0])
objrotmat = dc.strToMat4(resultrow[1])
objpos = objrotmat.getRow3(3)
base.changeLookAt(lookatp=[objpos[0], objpos[1], objpos[2]])
# show object
geom = pg.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom)
star = NodePath('obj')
star.attachNewNode(node)
star.setColor(Vec4(.77, .67, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
sql = "SELECT tabletopgrips.rotmat, tabletopgrips.jawwidth FROM tabletopgrips WHERE \
tabletopgrips.idtabletopplacements=%d" % idtabletopplacements
result = gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
tmphnd = self.handpkg.newHandNM(hndcolor=[0, 1, 0, .1])
tmphnd.setMat(pandanpmat4=hndrotmat)
tmphnd.setJawwidth(hndjawwidth)
tmphnd.reparentTo(base.render)
def grpshow(self, base, gdb):
sql = "SELECT tabletopplacements.idtabletopplacements, tabletopplacements.rotmat \
FROM tabletopplacements,freetabletopplacement,object,angle WHERE \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s' AND \
tabletopplacements.idangle = angle.idangle AND \
freetabletopplacement.idfreetabletopplacement = %d AND angle.value = %d" % (self.dbobjname, 1, 45)
result = gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
idtabletopplacements = int(resultrow[0])
objrotmat = dc.strToMat4(resultrow[1])
objpos = objrotmat.getRow3(3)
base.changeLookAt(lookatp=[objpos[0],objpos[1],objpos[2]])
# show object
geom = pg.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom)
star = NodePath('obj')
star.attachNewNode(node)
star.setColor(Vec4(.77,.67,0,1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
sql = "SELECT tabletopgrips.rotmat, tabletopgrips.jawwidth FROM tabletopgrips WHERE \
tabletopgrips.idtabletopplacements=%d" % idtabletopplacements
result = gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
tmphnd = self.handpkg.newHandNM(hndcolor=[0, 1, 0, .1])
tmphnd.setMat(pandanpmat4 = hndrotmat)
tmphnd.setJawwidth(hndjawwidth)
tmphnd.reparentTo(base.render)
def grpsshow(self, base):
sql = "SELECT dropworkcellgrip.rotmat, dropworkcellgrip.jawwidth FROM dropworkcellgrip WHERE \
dropworkcellgrip.iddropstablepos=%d" % self.dbidstablepos
result = self.gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
#tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .1])
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, 0.7])
tmprtq85.setMat(hndrotmat)
tmprtq85.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmprtq85.reparentTo(base.render)
def loadDropFreeGrip(self, objname, handname="rtq85", idstablepos=1):
"""
load self.dropfreegripid, etc. from mysqldatabase
:param handname which hand to use, rtq85 by default
:return: a list of [freegripid, freegripcontacts, freegripnormals, freegriprotmats, freegripjawwidth]
author: jiayao
date: 20170810
"""
print "loadDropFreeGrip idstablepos", idstablepos
freegripid = []
freegripcontacts = []
freegripnormals = []
freegriprotmats = []
freegripjawwidth = []
freeairgripid = []
# access to db
sql = "SELECT freegrip.dropfreegrip.iddropfreegrip, freegrip.dropfreegrip.contactpnt0, freegrip.dropfreegrip.contactpnt1, \
freegrip.dropfreegrip.contactnormal0, freegrip.dropfreegrip.contactnormal1, freegrip.dropfreegrip.rotmat, \
freegrip.dropfreegrip.jawwidth,dropfreegrip.idfreeairgrip FROM freegrip.dropfreegrip, hand, object\
WHERE freegrip.dropfreegrip.idobject = object.idobject AND object.name like '%s' \
AND dropfreegrip.idhand = hand.idhand AND hand.name like '%s' \
AND dropfreegrip.iddropstablepos = %d \
" % (objname, handname, idstablepos)
data = self.execute(sql)
if len(data) != 0:
for i in range(len(data)):
freegripid.append(int(data[i][0]))
freegripcontacts.append(
[dc.strToV3(data[i][1]),
dc.strToV3(data[i][2])])
freegripnormals.append(
[dc.strToV3(data[i][3]),
dc.strToV3(data[i][4])])
freegriprotmats.append(dc.strToMat4(data[i][5]))
freegripjawwidth.append(float(data[i][6]))
freeairgripid.append(int(data[i][7]))
return [
freegripid, freegripcontacts, freegripnormals, freegriprotmats,
freegripjawwidth, freeairgripid
]
else:
print "no DropFreeGrip select"
return None
def saveToDB(self, positionlist, gdb, discretesize=8.0):
"""
:param positionlist: a list of positions to place the object one the table
:param discretesize: the discretization of rotation angles around z axis
:return:
author: weiwei
date: 20161215, osaka
"""
# save discretiezed angle
sql = "SELECT * FROM angle"
result = gdb.execute(sql)
if len(result) == 0:
sql = "INSERT INTO angle(value) VALUES "
for i in range(discretesize):
sql += "("+str(360*i*1.0/discretesize)+"), "
sql = sql[:-2]+";"
gdb.execute(sql)
else:
print "Angles already set!"
# save tabletopplacements
sql = "SELECT idtabletopplacements FROM tabletopplacements,freetabletopplacement,object WHERE \
tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
# 1) select the freetabletopplacement
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
result = np.asarray(result)
idfreelist = [int(x) for x in result[:, 0]]
rotmatfreelist = [dc.strToMat4(x) for x in result[:, 1]]
# 2) select the angle
sql = "SELECT angle.idangle,angle.value FROM angle"
result = np.asarray(gdb.execute(sql))
idanglelist = [int(x) for x in result[:, 0]]
anglevaluelist = [float(x) for x in result[:, 1]]
# 3) save to database
sql = "INSERT INTO tabletopplacements(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES "
for ttoppos in positionlist:
ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2])
for idfree, rotmatfree in zip(idfreelist, rotmatfreelist):
for idangle, anglevalue in zip(idanglelist, anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2]+";"
gdb.execute(sql)
else:
print "Tabletopplacements already exist!"
# save tabletopgrips
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT tabletopgrips.idtabletopgrips FROM tabletopgrips,freeairgrip,object WHERE \
tabletopgrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \
freeairgrip.idobject=object.idobject AND object.name LIKE '%s' AND \
freeairgrip.idhand = %d" % (self.dbobjname, idhand)
result = gdb.execute(sql)
if len(result) == 0:
sql = "SELECT freetabletopplacement.idfreetabletopplacement \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
for idfree in result:
idfree = int(idfree[0])
sql = "SELECT tabletopplacements.idtabletopplacements, \
tabletopplacements.tabletopposition, angle.value,\
freetabletopgrip.contactpoint0, freetabletopgrip.contactpoint1, \
freetabletopgrip.contactnormal0, freetabletopgrip.contactnormal1, \
freetabletopgrip.rotmat, freetabletopgrip.jawwidth, freetabletopgrip.idfreeairgrip \
FROM tabletopplacements,freetabletopplacement,freetabletopgrip,freeairgrip,angle WHERE \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
tabletopplacements.idangle = angle.idangle AND \
freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = %d AND \
freetabletopplacement.idfreetabletopplacement = %d" % (idhand, idfree)
result1 = gdb.execute(sql)
if len(result1) == 0:
# no grasp availalbe?
continue
# sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, \
# contactnormal1, rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES "
if len(result1) > 20000:
result1 = result1[0::int(len(result1)/20000.0)]
result1 = np.asarray(result1)
idtabletopplacementslist = [int(x) for x in result1[:,0]]
tabletoppositionlist = [dc.strToV3(x) for x in result1[:,1]]
rotanglelist = [float(x) for x in result1[:,2]]
freegripcontactpoint0list = [dc.strToV3(x) for x in result1[:,3]]
freegripcontactpoint1list = [dc.strToV3(x) for x in result1[:,4]]
freegripcontactnormal0list = [dc.strToV3(x) for x in result1[:,5]]
freegripcontactnormal1list = [dc.strToV3(x) for x in result1[:,6]]
freegriprotmatlist = [dc.strToMat4(x) for x in result1[:,7]]
freegripjawwidthlist = [float(x) for x in result1[:,8]]
freegripidlist = [int(x) for x in result1[:,9]]
for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \
freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \
tabletoppositionlist, rotanglelist, freegripcontactpoint0list, freegripcontactpoint1list, \
freegripcontactnormal0list, freegripcontactnormal1list, freegriprotmatlist, freegripjawwidthlist, \
freegripidlist):
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
ttpcct0 = rotmat4.xformPoint(cct0)
ttpcct1 = rotmat4.xformPoint(cct1)
ttpcctn0 = rotmat4.xformVec(cctn0)
ttpcctn1 = rotmat4.xformVec(cctn1)
ttpgriprotmat = freegriprotmat*rotmat4
sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES \
('%s', '%s', '%s', '%s', '%s', '%s', %d, %d) " % \
(dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \
dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements)
gdb.execute(sql)
else:
print "Tabletopgrips already exist!"
print "Save to DB done!"
#
# objstart.reparentTo(base.render)
# objend.reparentTo(base.render)
# #
# # # import time
# # # tic = time.clock()
#regrip.findshortestpath(startrotmat4, goalrotmat4, base)
dbobjname = os.path.splitext(os.path.basename(objpath))[0]
sql = "SELECT startgoal.idstartgoal,startgoal.rotmat,startgoal.idfreetabletopplacement FROM startgoal,freetabletopplacement,object WHERE \
startgoal.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % dbobjname
result = gdb.execute(sql)
result = np.asarray(result)
idsglist = [int(x) for x in result[:, 0]]
sgrotmatlist = [dc.strToMat4(x) for x in result[:, 1]]
idfreeplacementlist = [int(x) for x in result[:, 2]]
for idstart, start in zip(idsglist, sgrotmatlist):
for idgoal, goal in zip(idsglist, sgrotmatlist):
# print idstart, idgoal
if idstart == 2 and idgoal == 1:
print start, goal
startrotmat4 = start
goalrotmat4 = goal
regrip.findshortestpath(start, goal, base)
break
#
objstart = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
objstart.setMat(startrotmat4)
objend = pg.genObjmnp(objpath, color=Vec4(.3, .3, 0, .5))
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)
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 saveToDB(self, positionlist, gdb, discretesize=4):
"""
:param positionlist: a list of positions to place the object one the table
:param discretesize: the discretization of rotation angles around z axis
:return:
author: weiwei
date: 20161215, osaka
"""
# save discretiezed angle
sql = "SELECT * FROM angle"
result = gdb.execute(sql)
if len(result) == 0:
sql = "INSERT INTO angle(value) VALUES "
for i in range(discretesize):
sql += "(" + str(360 * i * 1.0 / discretesize) + "), "
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "Angles already set!"
# save tabletopplacements
sql = "SELECT idstartgoal FROM startgoal,freetabletopplacement,object WHERE \
startgoal.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
# 1) select the freetabletopplacement
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
result = np.asarray(result)
idfreelist = [int(x) for x in result[:, 0]]
rotmatfreelist = [dc.strToMat4(x) for x in result[:, 1]]
# 2) select the angle
sql = "SELECT angle.idangle,angle.value FROM angle"
result = np.asarray(gdb.execute(sql))
idanglelist = [int(x) for x in result[:, 0]]
anglevaluelist = [float(x) for x in result[:, 1]]
# 3) save to database
sql = "INSERT INTO startgoal(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES "
for ttoppos in positionlist:
ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2])
for idfree, rotmatfree in zip(idfreelist, rotmatfreelist):
for idangle, anglevalue in zip(idanglelist,
anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "startgoal already exist!"
print "Save to DB done!"
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 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 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 updateDBwithIK(self, gdb, robot, armname = 'rgt'):
"""
:param gdb:
:param robot:
:param armname: the name of the arm used rgt or lft
:param rethandx: the distance of retract along handx direction, default 50mm
:param retworldz: the distance of retract along worldz direction, default 50mm
:param retworlda: the distance of retract along assembly/approaching direction in the world, default 50mm
:return:
author: weiwei
date: 20170111
"""
# load idarm
idarm = gdb.loadIdArm(armname)
# load retraction distances
rethandx, retworldz, retworlda, worldz = gdb.loadIKRet()
# worlda is default for the general grasps on table top
# for assembly at start and goal, worlda is computed by assembly planner
worlda = Vec3(0,0,1)
# select idrobot
idrobot = gdb.loadIdRobot(robot)
feasibility = {}
feasibility_handx = {}
feasibility_handxworldz = {}
feasibility_worlda = {}
feasibility_worldaworldz = {}
sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \
tabletopgrips.rotmat FROM tabletopgrips, tabletopplacements, freetabletopplacement, object WHERE \
tabletopgrips.idtabletopplacements = tabletopplacements.idtabletopplacements AND \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the tabletopgrips first!")
idcounter = 0
tic = time.clock()
for resultrow in result:
print idcounter*1.0/len(result)
idcounter += 1
toc = time.clock()
print toc-tic
ttgsid = int(resultrow[0])
ttgscct0 = dc.strToV3(resultrow[1])
ttgscct1 = dc.strToV3(resultrow[2])
ttgsrotmat = dc.strToMat4(resultrow[3])
ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + worldz*retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + worlda*retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ worldz*retworldz
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
if robot.numik(ttgsfgrcenternp, ttgsrotmat3np, armname) is not None:
feasibility[ttgsid] = 'True'
else:
feasibility[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_handx, ttgsrotmat3np, armname) is not None:
feasibility_handx[ttgsid] = 'True'
else:
feasibility_handx[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_handxworldz, ttgsrotmat3np, armname) is not None:
feasibility_handxworldz[ttgsid] = 'True'
else:
feasibility_handxworldz[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_worlda, ttgsrotmat3np, armname) is not None:
feasibility_worlda[ttgsid] = 'True'
else:
feasibility_worlda[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np, armname) is not None:
feasibility_worldaworldz[ttgsid] = 'True'
else:
feasibility_worldaworldz[ttgsid] = 'False'
# insert ik table
sql = "INSERT IGNORE INTO ik(idrobot, idarm, idtabletopgrips, feasibility, feasibility_handx, feasibility_handxworldz, \
feasibility_worlda, feasibility_worldaworldz) VALUES (%d, %d, %d, '%s', '%s', '%s', '%s', '%s')" % \
(idrobot, idarm, ttgsid, feasibility[ttgsid], feasibility_handx[ttgsid], feasibility_handxworldz[ttgsid], \
feasibility_worlda[ttgsid], feasibility_worldaworldz[ttgsid])
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 updateDBwithIK(self, gdb, robot, armname='rgt'):
"""
:param gdb:
:param robot:
:param armname: the name of the arm used rgt or lft
:param rethandx: the distance of retract along handx direction, default 50mm
:param retworldz: the distance of retract along worldz direction, default 50mm
:param retworlda: the distance of retract along assembly/approaching direction in the world, default 50mm
:return:
author: weiwei
date: 20170111
"""
# load idarm
idarm = gdb.loadIdArm(armname)
# load retraction distances
rethandx, retworldz, retworlda, worldz = gdb.loadIKRet()
# worlda is default for the general grasps on table top
# for assembly at start and goal, worlda is computed by assembly planner
worlda = Vec3(0, 0, 1)
# select idrobot
idrobot = gdb.loadIdRobot(robot)
feasibility = {}
feasibility_handx = {}
feasibility_handxworldz = {}
feasibility_worlda = {}
feasibility_worldaworldz = {}
sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \
tabletopgrips.rotmat FROM tabletopgrips, tabletopplacements, freetabletopplacement, object WHERE \
tabletopgrips.idtabletopplacements = tabletopplacements.idtabletopplacements AND \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the tabletopgrips first!")
idcounter = 0
tic = time.clock()
for resultrow in result:
print idcounter * 1.0 / len(result)
idcounter += 1
toc = time.clock()
print toc - tic
ttgsid = int(resultrow[0])
ttgscct0 = dc.strToV3(resultrow[1])
ttgscct1 = dc.strToV3(resultrow[2])
ttgsrotmat = dc.strToMat4(resultrow[3])
ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx * rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + worldz * retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + worlda * retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda + worldz * retworldz
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
if robot.numik(ttgsfgrcenternp, ttgsrotmat3np,
armname) is not None:
feasibility[ttgsid] = 'True'
else:
feasibility[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_handx, ttgsrotmat3np,
armname) is not None:
feasibility_handx[ttgsid] = 'True'
else:
feasibility_handx[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_handxworldz, ttgsrotmat3np,
armname) is not None:
feasibility_handxworldz[ttgsid] = 'True'
else:
feasibility_handxworldz[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_worlda, ttgsrotmat3np,
armname) is not None:
feasibility_worlda[ttgsid] = 'True'
else:
feasibility_worlda[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np,
armname) is not None:
feasibility_worldaworldz[ttgsid] = 'True'
else:
feasibility_worldaworldz[ttgsid] = 'False'
# insert ik table
sql = "INSERT IGNORE INTO ik(idrobot, idarm, idtabletopgrips, feasibility, feasibility_handx, feasibility_handxworldz, \
feasibility_worlda, feasibility_worldaworldz) VALUES (%d, %d, %d, '%s', '%s', '%s', '%s', '%s')" % \
(idrobot, idarm, ttgsid, feasibility[ttgsid], feasibility_handx[ttgsid], feasibility_handxworldz[ttgsid], \
feasibility_worlda[ttgsid], feasibility_worldaworldz[ttgsid])
gdb.execute(sql)
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 __loadGripsToBuildGraph(self, armname="rgt"):
"""
load tabletopgrips
retraction distance are also loaded from database
:param robot: an robot defined in robotsim.hrp5 or robotsim.nextage
:param gdb: an object of the database.GraspDB class
:param idarm: value = 1 "lft" or 2 "rgt", which arm to use
:return:
author: weiwei
date: 20170112
"""
# load idarm
idarm = gdb.loadIdArm(armname)
# get the global grip ids
# and prepare the global edges
# for each globalgripid, find all its tabletopids (pertaining to placements)
globalidsedges = {}
sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan freeairgrip first!")
for ggid in result:
globalidsedges[str(ggid[0])] = []
self.globalgripids.append(ggid[0])
sql = "SELECT tabletopplacements.idtabletopplacements, angle.value, \
tabletopplacements.idfreetabletopplacement, tabletopplacements.tabletopposition, \
tabletopplacements.rotmat FROM \
tabletopplacements,freetabletopplacement,angle,object WHERE \
tabletopplacements.idangle=angle.idangle AND \
tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND \
object.name LIKE '%s' AND angle.value IN (0.0, 45.0, 90.0, 135.0, 180.0, 225.0, 270.0, 315.0)" \
% self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
tpsrows = np.array(result)
# nubmer of discreted rotation
self.angles = list(set(map(float, tpsrows[:, 1])))
# for plotting
self.fttpsids = list(set(map(int, tpsrows[:, 2])))
self.nfttps = len(self.fttpsids)
idrobot = self.gdb.loadIdRobot(self.robot)
for i, idtps in enumerate(tpsrows[:, 0]):
sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \
tabletopgrips.rotmat, tabletopgrips.jawwidth, tabletopgrips.idfreeairgrip \
FROM tabletopgrips,ik,freeairgrip,hand WHERE tabletopgrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = hand.idhand AND\
tabletopgrips.idtabletopgrips=ik.idtabletopgrips AND \
tabletopgrips.idtabletopplacements = %d AND ik.idrobot=%d AND \
ik.feasibility='True' AND ik.feasibility_handx='True' AND ik.feasibility_handxworldz='True' \
AND ik.feasibility_worlda='True' AND ik.feasibility_worldaworldz='True' AND ik.idarm = %d \
AND hand.name LIKE '%s'" \
% (int(idtps), idrobot, idarm, self.handpkg.getHandName())
resultttgs = self.gdb.execute(sql)
if len(resultttgs) == 0:
continue
localidedges = []
for ttgsrow in resultttgs:
ttgsid = int(ttgsrow[0])
ttgscct0 = dc.strToV3(ttgsrow[1])
ttgscct1 = dc.strToV3(ttgsrow[2])
ttgsrotmat = dc.strToMat4(ttgsrow[3])
ttgsjawwidth = float(ttgsrow[4])
ttgsidfreeair = int(ttgsrow[5])
ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx * self.rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz * self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda * self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda + self.worldz * self.retworldz
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(
ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(
ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
objrotmat4 = dc.strToMat4(tpsrows[:, 4][i])
objrotmat4worlda = Mat4(objrotmat4)
objrotmat4worlda.setRow(
3,
objrotmat4.getRow3(3) + self.worlda * self.retworlda)
objrotmat4worldaworldz = Mat4(objrotmat4worlda)
objrotmat4worldaworldz.setRow(
3,
objrotmat4worlda.getRow3(3) +
self.worldz * self.retworldz)
self.regg.add_node(
armname + str(ttgsid),
fgrcenter=ttgsfgrcenternp,
fgrcenterhandx=ttgsfgrcenternp_handx,
fgrcenterhandxworldz=ttgsfgrcenternp_handxworldz,
fgrcenterworlda=ttgsfgrcenternp_worlda,
fgrcenterworldaworldz=ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth,
hndrotmat3np=ttgsrotmat3np,
globalgripid=ttgsidfreeair,
freetabletopplacementid=int(tpsrows[:, 2][i]),
tabletopplacementrotmat=objrotmat4,
tabletopplacementrotmathandx=objrotmat4,
tabletopplacementrotmathandxworldz=objrotmat4,
tabletopplacementrotmatworlda=objrotmat4worlda,
tabletopplacementrotmatworldaworldz=
objrotmat4worldaworldz,
angle=float(tpsrows[:, 1][i]),
tabletopposition=dc.strToV3(tpsrows[:, 3][i]))
globalidsedges[str(ttgsidfreeair)].append(armname +
str(ttgsid))
localidedges.append(armname + str(ttgsid))
# print list(itertools.combinations(ttgrows[:,0], 2))
for edge in list(itertools.combinations(localidedges, 2)):
self.regg.add_edge(*edge, weight=1, edgetype='transit')
if len(globalidsedges) == 0:
raise ValueError("Plan tabletopgrips first!")
for globalidedgesid in globalidsedges:
for edge in list(
itertools.combinations(globalidsedges[globalidedgesid],
2)):
self.regg.add_edge(*edge, weight=1, edgetype='transfer')
# gen plot pos
# biggest circle: grips; big circle: rotation; small circle: placements
radiusplacement = 30
radiusrot = 6
radiusgrip = 1
xyplacementspos = {}
xydiscreterotspos = {}
xyzglobalgrippos = {}
for i, ttpsid in enumerate(self.fttpsids):
xydiscreterotspos[ttpsid] = {}
xyzglobalgrippos[ttpsid] = {}
xypos = [
radiusplacement * math.cos(2 * math.pi / self.nfttps * i),
radiusplacement * math.sin(2 * math.pi / self.nfttps * i)
]
xyplacementspos[ttpsid] = xypos
for j, anglevalue in enumerate(self.angles):
xyzglobalgrippos[ttpsid][anglevalue] = {}
xypos = [
radiusrot * math.cos(anglevalue),
radiusrot * math.sin(anglevalue)
]
xydiscreterotspos[ttpsid][anglevalue] = \
[xyplacementspos[ttpsid][0] + xypos[0], xyplacementspos[ttpsid][1] + xypos[1]]
for k, globalgripid in enumerate(self.globalgripids):
xypos = [
radiusgrip *
math.cos(2 * math.pi / len(self.globalgripids) * k),
radiusgrip *
math.sin(2 * math.pi / len(self.globalgripids) * k)
]
xyzglobalgrippos[ttpsid][anglevalue][globalgripid] = \
[xydiscreterotspos[ttpsid][anglevalue][0] + xypos[0],
xydiscreterotspos[ttpsid][anglevalue][1] + xypos[1], 0]
for nid in self.regg.nodes():
fttpid = self.regg.node[nid]['freetabletopplacementid']
anglevalue = self.regg.node[nid]['angle']
ggid = self.regg.node[nid]['globalgripid']
tabletopposition = self.regg.node[nid]['tabletopposition']
xyzpos = map(add, xyzglobalgrippos[fttpid][anglevalue][ggid], [
tabletopposition[0], tabletopposition[1], tabletopposition[2]
])
self.gnodesplotpos[nid] = xyzpos[:2]
def __loadGripsToBuildGraph(self, armname = "rgt"):
"""
load tabletopgrips
retraction distance are also loaded from database
:param robot: an robot defined in robotsim.hrp5 or robotsim.nextage
:param gdb: an object of the database.GraspDB class
:param idarm: value = 1 "lft" or 2 "rgt", which arm to use
:return:
author: weiwei
date: 20170112
"""
# load idarm
idarm = self.gdb.loadIdArm(armname)
idhand = self.gdb.loadIdHand(self.handname)
# get the global grip ids
# and prepare the global edges
# for each globalgripid, find all its tabletopids (pertaining to placements)
globalidsedges = {}
sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand = %d" % (self.dbobjname, idhand)
# sql = "SELECT dropfreegrip.iddropfreegrip FROM dropfreegrip,object WHERE dropfreegrip.idobject=object.idobject AND \
# object.name LIKE '%s' AND dropfreegrip.idhand = %d" % (self.dbobjname, idhand)
# sql = "SELECT dropworkcellgrip.iddropworkcellgrip FROM dropworkcellgrip,object WHERE dropworkcellgrip.idobject=object.idobject AND \
# object.name LIKE '%s' AND dropworkcellgrip.idhand = %d" % (self.dbobjname, idhand)
result = self.gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan freeairgrip first!")
for ggid in result:
globalidsedges[str(ggid[0])] = []
self.globalgripids.append(ggid[0])
sql = "SELECT dropstablepos.iddropstablepos,dropstablepos.rot,dropstablepos.pos,angle_drop.value FROM \
dropstablepos,object,angle_drop WHERE \
dropstablepos.idobject=object.idobject AND \
object.name LIKE '%s' " % self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
tpsrows = np.array(result)
#self.angles = list([0.0])
self.angles = list(set(map(float, tpsrows[:, 3])))
# for plotting
self.fttpsids = list(set(map(int, tpsrows[:, 0])))
self.nfttps = len(self.fttpsids)
idrobot = self.gdb.loadIdRobot(self.robot)
for i, idtps in enumerate(tpsrows[:,0]):
sql = "SELECT dropworkcellgrip.iddropworkcellgrip, dropworkcellgrip.contactpnt0, dropworkcellgrip.contactpnt1, \
dropworkcellgrip.rotmat, dropworkcellgrip.jawwidth ,dropworkcellgrip.idfreeairgrip\
FROM dropworkcellgrip,dropfreegrip,freeairgrip,ik_drop\
WHERE \
dropworkcellgrip.iddropstablepos = %d \
AND dropworkcellgrip.iddropworkcellgrip=ik_drop.iddropworkcellgrip AND ik_drop.idrobot=%d AND ik_drop.idarm = %d AND\
ik_drop.feasibility='True' AND ik_drop.feasibility_handx='True' AND ik_drop.feasibility_handxworldz='True' \
AND ik_drop.feasibility_worlda='True' AND ik_drop.feasibility_worldaworldz='True' \
AND dropworkcellgrip.idfreeairgrip = freeairgrip.idfreeairgrip \
AND dropworkcellgrip.idhand = % d AND dropworkcellgrip.iddropfreegrip = dropfreegrip.iddropfreegrip " \
% (int(idtps),idrobot, idarm, idhand)
resultttgs = self.gdb.execute(sql)
if len(resultttgs)==0:
print "no result"
continue
localidedges = []
for ttgsrow in resultttgs:
ttgsid = int(ttgsrow[0])
ttgscct0 = dc.strToV3(ttgsrow[1])
ttgscct1 = dc.strToV3(ttgsrow[2])
ttgsrotmat = dc.strToMat4(ttgsrow[3])
ttgsjawwidth = float(ttgsrow[4])
ttgsidfreeair = int(ttgsrow[5])
ttgsfgrcenter = (ttgscct0+ttgscct1)/2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
objrotmat4 = pg.npToMat4(np.transpose(pg.mat3ToNp(dc.strToMat3(tpsrows[:, 1][i]))),
pg.v3ToNp(dc.strToV3(tpsrows[:, 2][i])))
objrotmat4worlda = Mat4(objrotmat4)
objrotmat4worlda.setRow(3, objrotmat4.getRow3(3)+self.worlda*self.retworlda)
objrotmat4worldaworldz = Mat4(objrotmat4worlda)
objrotmat4worldaworldz.setRow(3, objrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node('mid' + str(ttgsid), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz,
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair,
#freetabletopplacementid = int(tpsrows[:,2][i]),
freetabletopplacementid=int(tpsrows[:, 0][i]),
tabletopplacementrotmat = objrotmat4,
tabletopplacementrotmathandx = objrotmat4,
tabletopplacementrotmathandxworldz = objrotmat4,
tabletopplacementrotmatworlda = objrotmat4worlda,
tabletopplacementrotmatworldaworldz = objrotmat4worldaworldz,
angle = float(tpsrows[:,3][i]),
tabletopposition = dc.strToV3(tpsrows[:,2][i]))
print "str(ttgsidfreeair),str(ttgsid)",str(ttgsidfreeair),str(ttgsid)
globalidsedges[str(ttgsidfreeair)].append('mid' + str(ttgsid))
localidedges.append('mid' + str(ttgsid))
for edge in list(itertools.combinations(localidedges, 2)):
self.regg.add_edge(*edge, weight=1, edgetype = 'transit')
#toc = time.clock()
#print "(toc - tic2)", (toc - tic)
if len(globalidsedges) == 0:
raise ValueError("Plan tabletopgrips first!")
#tic = time.clock()
for globalidedgesid in globalidsedges:
for edge in list(itertools.combinations(globalidsedges[globalidedgesid], 2)):
self.regg.add_edge(*edge, weight=1, edgetype = 'transfer')
#toc = time.clock()
#print "(toc - tic3)", (toc - tic)
else:
print ('No placements planned!')
assert('No placements planned!')
def freegripRotMove(objname,handname):
"""
:param objname:
:return:
for each dropstablepos
caculate its grips after rot and move
and save to database dropfreegrip to remove the hand around workcell next
mention that the dropstablepos rot,pos
rotmat=(np.transpose(rot),pos)
"""
gdb = db.GraspDB()
idhand = gdb.loadIdHand(handname)
idobject=gdb.loadIdObject(objname)
sql = "SELECT dropstablepos.iddropstablepos\
FROM dropstablepos, object \
WHERE dropstablepos.idobject = object.idobject AND object.name like '%s'" % (objname)
result = gdb.execute(sql)
print result
if len(result) == 0:
print "no DropStablePos select"
return None
for idfree in result:
idfree = int(idfree[0])
sql = "SELECT dropstablepos.iddropstablepos, \
dropstablepos.pos, dropstablepos.rot,\
freeairgrip.contactpnt0, freeairgrip.contactpnt1, \
freeairgrip.contactnormal0, freeairgrip.contactnormal1, \
freeairgrip.rotmat, freeairgrip.jawwidth, freeairgrip.idfreeairgrip \
FROM dropstablepos,freeairgrip WHERE \
freeairgrip.idhand = %d AND \
dropstablepos.iddropstablepos = %d" % (idhand, idfree)
result1 = gdb.execute(sql)
if len(result1) == 0:
print "no free air grasp availalbe"
continue
if len(result1) > 20000:
result1 = result1[0::int(len(result1) / 20000.0)]
result1 = np.asarray(result1)
idtabletopplacementslist = [int(x) for x in result1[:, 0]]
tabletoppositionlist = [dc.strToV3(x) for x in result1[:, 1]]
#rotanglelist = [float(x) for x in result1[:, 2]]
rotanglelist = [dc.strToMat3(x) for x in result1[:, 2]]
freegripcontactpoint0list = [dc.strToV3(x) for x in result1[:, 3]]
freegripcontactpoint1list = [dc.strToV3(x) for x in result1[:, 4]]
freegripcontactnormal0list = [dc.strToV3(x) for x in result1[:, 5]]
freegripcontactnormal1list = [dc.strToV3(x) for x in result1[:, 6]]
freegriprotmatlist = [dc.strToMat4(x) for x in result1[:, 7]]
freegripjawwidthlist = [float(x) for x in result1[:, 8]]
freegripidlist = [int(x) for x in result1[:, 9]]
for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \
freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \
tabletoppositionlist, rotanglelist, \
freegripcontactpoint0list, freegripcontactpoint1list, \
freegripcontactnormal0list, freegripcontactnormal1list,
freegriprotmatlist, freegripjawwidthlist, \
freegripidlist):
rotmat4=pg.npToMat4(np.transpose(pg.mat3ToNp(rotangle)), ttoppos)
ttpcct0 = rotmat4.xformPoint(cct0)
ttpcct1 = rotmat4.xformPoint(cct1)
ttpcctn0 = rotmat4.xformVec(cctn0)
ttpcctn1 = rotmat4.xformVec(cctn1)
ttpgriprotmat = freegriprotmat * rotmat4
#ttpgriprotmat = rotmat4*freegriprotmat
sql = "INSERT INTO dropfreegrip(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreeairgrip, iddropstablepos,idhand,idobject) VALUES \
('%s', '%s', '%s', '%s', '%s', '%s', %d, %d,%d,%d) " % \
(dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \
dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements, idhand, idobject)
gdb.execute(sql)
def saveToDB(self, positionlist, gdb, discretesize=4.0):
"""
:param positionlist: a list of positions to place the object one the table
:param discretesize: the discretization of rotation angles around z axis
:return:
author: weiwei
date: 20161215, osaka
"""
# save discretiezed angle
sql = "SELECT * FROM angle"
result = gdb.execute(sql)
if len(result) == 0:
sql = "INSERT INTO angle(value) VALUES "
for i in range(discretesize):
sql += "(" + str(360 * i * 1.0 / discretesize) + "), "
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "Angles already set!"
# save tabletopplacements
sql = "SELECT idtabletopplacements FROM tabletopplacements,freetabletopplacement,object WHERE \
tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
# 1) select the freetabletopplacement
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
result = np.asarray(result)
idfreelist = [int(x) for x in result[:, 0]]
rotmatfreelist = [dc.strToMat4(x) for x in result[:, 1]]
# 2) select the angle
sql = "SELECT angle.idangle,angle.value FROM angle"
result = np.asarray(gdb.execute(sql))
idanglelist = [int(x) for x in result[:, 0]]
anglevaluelist = [float(x) for x in result[:, 1]]
# 3) save to database
sql = "INSERT INTO tabletopplacements(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES "
for ttoppos in positionlist:
ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2])
for idfree, rotmatfree in zip(idfreelist, rotmatfreelist):
for idangle, anglevalue in zip(idanglelist,
anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "Tabletopplacements already exist!"
# save tabletopgrips
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT tabletopgrips.idtabletopgrips FROM tabletopgrips,freeairgrip,object WHERE \
tabletopgrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \
freeairgrip.idobject=object.idobject AND object.name LIKE '%s' AND \
freeairgrip.idhand = %d" % (self.dbobjname, idhand)
result = gdb.execute(sql)
if len(result) == 0:
sql = "SELECT freetabletopplacement.idfreetabletopplacement \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
for idfree in result:
idfree = int(idfree[0])
sql = "SELECT tabletopplacements.idtabletopplacements, \
tabletopplacements.tabletopposition, angle.value,\
freetabletopgrip.contactpoint0, freetabletopgrip.contactpoint1, \
freetabletopgrip.contactnormal0, freetabletopgrip.contactnormal1, \
freetabletopgrip.rotmat, freetabletopgrip.jawwidth, freetabletopgrip.idfreeairgrip \
FROM tabletopplacements,freetabletopplacement,freetabletopgrip,freeairgrip,angle WHERE \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
tabletopplacements.idangle = angle.idangle AND \
freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = %d AND \
freetabletopplacement.idfreetabletopplacement = %d" % (
idhand, idfree)
result1 = gdb.execute(sql)
if len(result1) == 0:
# no grasp availalbe?
continue
# sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, \
# contactnormal1, rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES "
if len(result1) > 20000:
result1 = result1[0::int(len(result1) / 20000.0)]
result1 = np.asarray(result1)
idtabletopplacementslist = [int(x) for x in result1[:, 0]]
tabletoppositionlist = [dc.strToV3(x) for x in result1[:, 1]]
rotanglelist = [float(x) for x in result1[:, 2]]
freegripcontactpoint0list = [
dc.strToV3(x) for x in result1[:, 3]
]
freegripcontactpoint1list = [
dc.strToV3(x) for x in result1[:, 4]
]
freegripcontactnormal0list = [
dc.strToV3(x) for x in result1[:, 5]
]
freegripcontactnormal1list = [
dc.strToV3(x) for x in result1[:, 6]
]
freegriprotmatlist = [dc.strToMat4(x) for x in result1[:, 7]]
freegripjawwidthlist = [float(x) for x in result1[:, 8]]
freegripidlist = [int(x) for x in result1[:, 9]]
for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \
freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \
tabletoppositionlist, rotanglelist, freegripcontactpoint0list, freegripcontactpoint1list, \
freegripcontactnormal0list, freegripcontactnormal1list, freegriprotmatlist, freegripjawwidthlist, \
freegripidlist):
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
ttpcct0 = rotmat4.xformPoint(cct0)
ttpcct1 = rotmat4.xformPoint(cct1)
ttpcctn0 = rotmat4.xformVec(cctn0)
ttpcctn1 = rotmat4.xformVec(cctn1)
ttpgriprotmat = freegriprotmat * rotmat4
sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES \
('%s', '%s', '%s', '%s', '%s', '%s', %d, %d) " % \
(dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \
dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements)
gdb.execute(sql)
else:
print "Tabletopgrips already exist!"
print "Save to DB done!"
def __loadGripsToBuildGraph(self, armname = "rgt"):
"""
load tabletopgrips
retraction distance are also loaded from database
:param robot: an robot defined in robotsim.hrp5 or robotsim.nextage
:param gdb: an object of the database.GraspDB class
:param idarm: value = 1 "lft" or 2 "rgt", which arm to use
:return:
author: weiwei
date: 20170112
"""
# load idarm
idarm = gdb.loadIdArm(armname)
# get the global grip ids
# and prepare the global edges
# for each globalgripid, find all its tabletopids (pertaining to placements)
globalidsedges = {}
sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan freeairgrip first!")
for ggid in result:
globalidsedges[str(ggid[0])] = []
self.globalgripids.append(ggid[0])
sql = "SELECT tabletopplacements.idtabletopplacements, angle.value, \
tabletopplacements.idfreetabletopplacement, tabletopplacements.tabletopposition, \
tabletopplacements.rotmat FROM \
tabletopplacements,freetabletopplacement,angle,object WHERE \
tabletopplacements.idangle=angle.idangle AND \
tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND \
object.name LIKE '%s' AND angle.value IN (0.0, 45.0, 90.0, 135.0, 180.0, 225.0, 270.0, 315.0)" \
% self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
tpsrows = np.array(result)
# nubmer of discreted rotation
self.angles = list(set(map(float, tpsrows[:,1])))
# for plotting
self.fttpsids = list(set(map(int, tpsrows[:,2])))
self.nfttps = len(self.fttpsids)
idrobot = self.gdb.loadIdRobot(self.robot)
for i, idtps in enumerate(tpsrows[:,0]):
sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \
tabletopgrips.rotmat, tabletopgrips.jawwidth, tabletopgrips.idfreeairgrip \
FROM tabletopgrips,ik,freeairgrip,hand WHERE tabletopgrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = hand.idhand AND\
tabletopgrips.idtabletopgrips=ik.idtabletopgrips AND \
tabletopgrips.idtabletopplacements = %d AND ik.idrobot=%d AND \
ik.feasibility='True' AND ik.feasibility_handx='True' AND ik.feasibility_handxworldz='True' \
AND ik.feasibility_worlda='True' AND ik.feasibility_worldaworldz='True' AND ik.idarm = %d \
AND hand.name LIKE '%s'" \
% (int(idtps), idrobot, idarm, self.handpkg.getHandName())
resultttgs = self.gdb.execute(sql)
if len(resultttgs)==0:
continue
localidedges = []
for ttgsrow in resultttgs:
ttgsid = int(ttgsrow[0])
ttgscct0 = dc.strToV3(ttgsrow[1])
ttgscct1 = dc.strToV3(ttgsrow[2])
ttgsrotmat = dc.strToMat4(ttgsrow[3])
ttgsjawwidth = float(ttgsrow[4])
ttgsidfreeair = int(ttgsrow[5])
ttgsfgrcenter = (ttgscct0+ttgscct1)/2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
objrotmat4 = dc.strToMat4(tpsrows[:,4][i])
objrotmat4worlda = Mat4(objrotmat4)
objrotmat4worlda.setRow(3, objrotmat4.getRow3(3)+self.worlda*self.retworlda)
objrotmat4worldaworldz = Mat4(objrotmat4worlda)
objrotmat4worldaworldz.setRow(3, objrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node(armname+str(ttgsid), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz,
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair, freetabletopplacementid = int(tpsrows[:,2][i]),
tabletopplacementrotmat = objrotmat4,
tabletopplacementrotmathandx = objrotmat4,
tabletopplacementrotmathandxworldz = objrotmat4,
tabletopplacementrotmatworlda = objrotmat4worlda,
tabletopplacementrotmatworldaworldz = objrotmat4worldaworldz,
angle = float(tpsrows[:,1][i]), tabletopposition = dc.strToV3(tpsrows[:,3][i]))
globalidsedges[str(ttgsidfreeair)].append(armname+str(ttgsid))
localidedges.append(armname+str(ttgsid))
# print list(itertools.combinations(ttgrows[:,0], 2))
for edge in list(itertools.combinations(localidedges, 2)):
self.regg.add_edge(*edge, weight=1, edgetype = 'transit')
if len(globalidsedges) == 0:
raise ValueError("Plan tabletopgrips first!")
for globalidedgesid in globalidsedges:
for edge in list(itertools.combinations(globalidsedges[globalidedgesid], 2)):
self.regg.add_edge(*edge, weight=1, edgetype = 'transfer')
# gen plot pos
# biggest circle: grips; big circle: rotation; small circle: placements
radiusplacement = 30
radiusrot = 6
radiusgrip = 1
xyplacementspos = {}
xydiscreterotspos = {}
xyzglobalgrippos = {}
for i, ttpsid in enumerate(self.fttpsids):
xydiscreterotspos[ttpsid] = {}
xyzglobalgrippos[ttpsid] = {}
xypos = [radiusplacement * math.cos(2 * math.pi / self.nfttps * i),
radiusplacement * math.sin(2 * math.pi / self.nfttps * i)]
xyplacementspos[ttpsid] = xypos
for j, anglevalue in enumerate(self.angles):
xyzglobalgrippos[ttpsid][anglevalue] = {}
xypos = [radiusrot * math.cos(anglevalue), radiusrot * math.sin(anglevalue)]
xydiscreterotspos[ttpsid][anglevalue] = \
[xyplacementspos[ttpsid][0] + xypos[0], xyplacementspos[ttpsid][1] + xypos[1]]
for k, globalgripid in enumerate(self.globalgripids):
xypos = [radiusgrip * math.cos(2 * math.pi / len(self.globalgripids) * k),
radiusgrip * math.sin(2 * math.pi / len(self.globalgripids) * k)]
xyzglobalgrippos[ttpsid][anglevalue][globalgripid] = \
[xydiscreterotspos[ttpsid][anglevalue][0] + xypos[0],
xydiscreterotspos[ttpsid][anglevalue][1] + xypos[1], 0]
for nid in self.regg.nodes():
fttpid = self.regg.node[nid]['freetabletopplacementid']
anglevalue = self.regg.node[nid]['angle']
ggid = self.regg.node[nid]['globalgripid']
tabletopposition = self.regg.node[nid]['tabletopposition']
xyzpos = map(add, xyzglobalgrippos[fttpid][anglevalue][ggid],
[tabletopposition[0], tabletopposition[1], tabletopposition[2]])
self.gnodesplotpos[nid] = xyzpos[:2]
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)
def freegripRotMove(self):
# self.freegripid = freeairgripdata[0]
# self.freegripcontacts = freeairgripdata[1]
# self.freegripnormals = freeairgripdata[2]
# self.freegriprotmats = freeairgripdata[3]
# self.freegripjawwidth = freeairgripdata[4]
idhand = 1
idobject = 1
gdb = db.GraspDB()
sql = "SELECT dropstablepos.iddropstablepos\
FROM dropstablepos, object \
WHERE dropstablepos.idobject = object.idobject AND object.name like '%s'" % (
self.dbobjname)
result = gdb.execute(sql)
print result
if len(result) == 0:
print "no DropStablePos select"
return None
for idfree in result:
idfree = int(idfree[0])
sql = "SELECT dropstablepos.iddropstablepos, \
dropstablepos.pos, dropstablepos.rot,\
freeairgrip.contactpnt0, freeairgrip.contactpnt1, \
freeairgrip.contactnormal0, freeairgrip.contactnormal1, \
freeairgrip.rotmat, freeairgrip.jawwidth, freeairgrip.idfreeairgrip \
FROM dropstablepos,freeairgrip WHERE \
freeairgrip.idhand = %d AND \
dropstablepos.iddropstablepos = %d" % (
idhand, idfree)
result1 = gdb.execute(sql)
if len(result1) == 0:
print "no free air grasp availalbe"
continue
if len(result1) > 20000:
result1 = result1[0::int(len(result1) / 20000.0)]
result1 = np.asarray(result1)
idtabletopplacementslist = [int(x) for x in result1[:, 0]]
tabletoppositionlist = [dc.strToV3(x) for x in result1[:, 1]]
# rotanglelist = [float(x) for x in result1[:, 2]]
rotanglelist = [dc.strToMat3(x) for x in result1[:, 2]]
freegripcontactpoint0list = [dc.strToV3(x) for x in result1[:, 3]]
freegripcontactpoint1list = [dc.strToV3(x) for x in result1[:, 4]]
freegripcontactnormal0list = [dc.strToV3(x) for x in result1[:, 5]]
freegripcontactnormal1list = [dc.strToV3(x) for x in result1[:, 6]]
freegriprotmatlist = [dc.strToMat4(x) for x in result1[:, 7]]
freegripjawwidthlist = [float(x) for x in result1[:, 8]]
freegripidlist = [int(x) for x in result1[:, 9]]
for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \
freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \
tabletoppositionlist, rotanglelist, \
freegripcontactpoint0list, freegripcontactpoint1list, \
freegripcontactnormal0list, freegripcontactnormal1list,
freegriprotmatlist, freegripjawwidthlist, \
freegripidlist):
# rotmat = rm.rodrigues([0, 0, 1], rotangle)
rotmat = rotangle
rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
ttoppos[0], ttoppos[1], ttoppos[2], 1)
# rotmat4 = pg.cvtMat4(rotmat, ttoppos)
# rotmat4=pg.cvtMat4(rotangle, ttoppos)
ttpcct0 = rotmat4.xformPoint(cct0)
ttpcct1 = rotmat4.xformPoint(cct1)
ttpcctn0 = rotmat4.xformVec(cctn0)
ttpcctn1 = rotmat4.xformVec(cctn1)
ttpgriprotmat = freegriprotmat * rotmat4
sql = "INSERT INTO dropfreegrip(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreeairgrip, iddropstablepos,idhand,idobject) VALUES \
('%s', '%s', '%s', '%s', '%s', '%s', %d, %d,%d,%d) " % \
(dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \
dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements, idhand, idobject)
gdb.execute(sql)