def __init__(self, objname, markerid=5):
self.markerid = markerid
self.objname = objname
self.gdb = db.GraspDB(database="nxt",
user="******",
password="******")
self.robot = nxtsim.NxtRobot()
self.rgthnd = sck918.Sck918(jawwidth=50,
hndcolor=(0.5, 0.5, 0.5, 1),
ftsensoroffset=0)
self.lfthnd = sck918.Sck918(jawwidth=50,
hndcolor=(0.5, 0.5, 0.5, 1),
ftsensoroffset=0)
self.robotmesh = nxtsimmesh.NxtMesh(rgthand=self.rgthnd,
lfthand=self.lfthnd)
self.robotball = nxtsimball.NxtBall()
self.robotik = nxtik
self.env = bldgfsettingnear.Env()
self.env.reparentTo(base.render)
self.objcm = self.env.loadobj(objname)
self.groove = self.env.loadobj("tenonhole_new_115.STL")
self.groove.setPos(595, 0, 973 - 76 + 75 + 80)
self.groove.setRPY(0, 90, -90)
self.groove.setColor(0, 0, 1, 0.4)
# self.groove.showLocalFrame()
self.groove.reparentTo(base.render)
self.obstaclecmlist = self.env.getstationaryobslist()
# for obstaclecdcm in self.obstaclecmlist:
# obstaclecdcm.showcn()
self.robot.goinitpose()
# self.rgtwatchpos = [400.0, -200.0, 1200.0]
# self.rgtwatchrotmat = [[0.09141122, -0.76823672, 0.63360582],
# [-0.99509199, -0.04625775, 0.08747659],
# [-0.03789355, -0.63849242, -0.76869468]]
# self.rgtwatchjs = self.robot.numik(self.rgtwatchpos, self.rgtwatchrotmat, "rgt")
#
# self.robot.movearmfk(self.rgtwatchjs, armname="rgt")
self.rbtmnp = self.robotmesh.genmnp(self.robot,
self.rgthnd.jawwidthopen,
self.lfthnd.jawwidthopen)
self.rbtmnp.reparentTo(base.render)
# uncomment the following commands to actuate the robot
self.nxtu = nxturx.NxtRobot(host="10.0.1.114:15005")
self.nxtu.goInitial()
# self.nxtu = nxturx.NxtCon()
# self.nxtu.setJointAnglesOfGroup('rarm', self.robot.initjnts[3:9], 5.0)
# self.nxtu.setJointAnglesOfGroup('larm', self.robot.initjnts[9:15], 5.0)
self.hc = hndcam.HndCam(rgtcamid=0, lftcamid=1)
# goallist, a list of 4x4 h**o numpy mat
self.goallist = []
self.objrenderlist = []
self.startobjcm = None
self.rbtmnpani = [None, None]
self.objmnpani = [None]
def saveToDB(self, gdb):
"""
save the result to mysqldatabase
:param gdb: is an object of the GraspDB class in the database package
:return:
author: jiayao
date: 20170809
"""
# save to database
gdb = db.GraspDB()
sql = "SELECT idobject FROM object WHERE name LIKE '%s'" % self.dbobjname
returnlist = gdb.execute(sql)
if len(returnlist) != 0:
idobject = returnlist[0][0]
else:
sql = "INSERT INTO object(name) VALUES('%s')" % self.dbobjname
idobject = gdb.execute(sql)
print "idobject", idobject
sql = "INSERT INTO freegrip.dropstablepos(idobject,rot,pos) VALUES('%d','%s','%s')" % \
(idobject,dc.mat3ToStr(self.rotmat),dc.LVecBase3fToStr(self.posmat))
gdb.execute(sql)
print "Drop saved !"
def saveToDB(self, gdb):
"""
save the result to mysqldatabase
:param gdb: is an object of the GraspDB class in the database package
:return:
author: jiayao
date: 20170811
"""
# save to database
gdb = db.GraspDB()
idhand = gdb.loadIdHand(self.handname)
idobject = 1
print len(self.gripcontacts)
for i in range(len(self.gripcontacts)):
sql = "INSERT INTO freegrip.dropfreegrip(idobject, contactpnt0, contactpnt1, \
contactnormal0, contactnormal1, rotmat, jawwidth, idhand,idstablepos) \
VALUES('%s', '%s', '%s', '%s', '%s', '%s', '%s', %d, %d)" % \
(idobject, dc.v3ToStr(self.gripcontacts[i][0]), dc.v3ToStr(self.gripcontacts[i][1]),
dc.v3ToStr(self.gripcontactnormals[i][0]), dc.v3ToStr(self.gripcontactnormals[i][1]),
dc.mat4ToStr(self.griprotmats[i]), str(self.gripjawwidth[i]), idhand, self.idstablepos)
gdb.execute(sql)
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 loadidstablepos():
gdb = db.GraspDB()
stablepos = []
sql = "SELECT freegrip.dropstablepos.iddropstablepos FROM freegrip.dropstablepos"
data = gdb.execute(sql)
if len(data) != 0:
for i in range(len(data)):
stablepos.append(int(data[i-1][0]))
return stablepos
else:
print "drop workcell to have stable pose first!"
def __init__(self, objname, markerid=5):
self.markerid = markerid
self.objname = objname
self.gdb = db.GraspDB(database="wrs",
user="******",
password="******")
self.robot = ur3dualsim.Ur3DualRobot()
self.rgthnd = rtq85nm.newHand(hndid="rgt")
self.lfthnd = rtq85nm.newHand(hndid="lft")
self.robotmesh = ur3dualsimmesh.Ur3DualMesh(rgthand=self.rgthnd,
lfthand=self.lfthnd)
self.robotball = ur3dualsimball.Ur3DualBall()
self.robotik = ur3dualik
self.env = bunrisettingfree.Env()
self.env.reparentTo(base.render)
self.objcm = self.env.loadobj(objname)
self.obstaclecmlist = self.env.getstationaryobslist()
for obstaclecdcm in self.obstaclecmlist:
obstaclecdcm.showcn()
self.robot.goinitpose()
self.rgtwatchpos = [400.0, -200.0, 1200.0]
self.rgtwatchrotmat = [[0.09141122, -0.76823672, 0.63360582],
[-0.99509199, -0.04625775, 0.08747659],
[-0.03789355, -0.63849242, -0.76869468]]
self.rgtwatchjs = self.robot.numik(self.rgtwatchpos,
self.rgtwatchrotmat, "rgt")
self.robot.movearmfk(self.rgtwatchjs, armname="rgt")
self.rbtmnp = self.robotmesh.genmnp(self.robot,
self.rgthnd.jawwidthopen,
self.lfthnd.jawwidthopen)
self.rbtmnp.reparentTo(base.render)
# uncomment the following commands to actuate the robot
# self.ur3u = ur3urx.Ur3DualUrx()
# self.ur3u.movejntssgl(self.robot.initjnts[3:9], 'rgt')
# self.ur3u.movejntssgl(self.robot.initjnts[9:15], 'lft')
# self.ur3u.movejntssgl(self.rgtwatchjs, 'rgt')
self.hc = cameras.HndCam(rgtcamid=0, lftcamid=1)
# goallist, a list of 4x4 h**o numpy mat
self.goallist = []
self.objrenderlist = []
self.startobjcm = None
self.rbtmnpani = [None, None]
self.objmnpani = [None]
def loadDropStablePos(self):
"""
load self.dropid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: jiayao
date: 20170810
"""
self.gdb = db.GraspDB()
dropstableposdata = self.gdb.loadDropStablePos(self.dbobjname)
if dropstableposdata is None:
raise ValueError("Plan the drop stable pos first!")
print "success load drop stable pos"
def __init__(self, objname, markerid=5):
self.markerid = markerid
self.objname = objname
self.gdb = db.GraspDB(database="nxt",
user="******",
password="******")
self.robot = nxtsim.NxtRobot()
self.rgthnd = rtq85nm.newHand(hndid="rgt", ftsensoroffset=0)
self.lfthnd = rtq85nm.newHand(hndid="lft", ftsensoroffset=0)
self.robotmesh = nxtsimmesh.NxtMesh(rgthand=self.rgthnd,
lfthand=self.lfthnd)
self.robotball = nxtsimball.NxtBall()
self.robotik = nxtik
self.env = bldgfsettingnear.Env()
self.env.reparentTo(base.render)
self.objcm = self.env.loadobj(objname)
self.obstaclecmlist = self.env.getstationaryobslist()
for obstaclecdcm in self.obstaclecmlist:
obstaclecdcm.showcn()
self.robot.goinitpose()
# self.rgtwatchpos = [400.0, -200.0, 1200.0]
# self.rgtwatchrotmat = [[0.09141122, -0.76823672, 0.63360582],
# [-0.99509199, -0.04625775, 0.08747659],
# [-0.03789355, -0.63849242, -0.76869468]]
# self.rgtwatchjs = self.robot.numik(self.rgtwatchpos, self.rgtwatchrotmat, "rgt")
#
# self.robot.movearmfk(self.rgtwatchjs, armname="rgt")
self.rbtmnp = self.robotmesh.genmnp(self.robot,
self.rgthnd.jawwidthopen,
self.lfthnd.jawwidthopen)
self.rbtmnp.reparentTo(base.render)
# uncomment the following commands to actuate the robot
# self.nxtu = nxturx.NxtCon()
# self.nxtu.setJointAnglesOfGroup('rarm', self.robot.initjnts[3:9], 5.0, True)
# self.nxtu.setJointAnglesOfGroup('larm', self.robot.initjnts[9:15], 5.0, True)
self.hc = hndcam.HndCam(rgtcamid=0, lftcamid=1)
# goallist, a list of 4x4 h**o numpy mat
self.goallist = []
self.objrenderlist = []
self.startobjcm = None
self.rbtmnpani = [None, None]
self.objmnpani = [None]
def loadDropStablePos(self):
"""
load self.dropid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: jiayao
date: 20170810
"""
self.gdb = db.GraspDB()
dropstableposdata = self.gdb.loadDropStablePos(self.dbobjname)
if dropstableposdata is None:
raise ValueError("Plan the drop stable pos first!")
objrotmat = dropstableposdata[1][self.dbidstablepos - 1]
objposmat = dropstableposdata[2][self.dbidstablepos - 1]
return [objrotmat, objposmat]
def saveToDB(self, gdb):
"""
save the result to mysqldatabase
:param gdb: is an object of the GraspDB class in the database package
:return:
author: weiwei
date: 20170110
"""
# save to database
gdb = db.GraspDB()
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT * FROM freeairgrip, object WHERE freeairgrip.idobject = object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand LIKE '%s'" % (
self.dbobjname, idhand)
result = gdb.execute(sql)
if not result:
sql = "SELECT idobject FROM object WHERE name LIKE '%s'" % self.dbobjname
returnlist = gdb.execute(sql)
if len(returnlist) != 0:
idobject = returnlist[0][0]
else:
sql = "INSERT INTO object(name) VALUES('%s')" % self.dbobjname
idobject = gdb.execute(sql)
print self.gripcontacts
for i in range(len(self.gripcontacts)):
sql = "INSERT INTO freeairgrip(idobject, contactpnt0, contactpnt1, \
contactnormal0, contactnormal1, rotmat, jawwidth, idhand) \
VALUES('%s', '%s', '%s', '%s', '%s', '%s', '%s', %d)" % \
(idobject, dc.v3ToStr(self.gripcontacts[i][0]), dc.v3ToStr(self.gripcontacts[i][1]),
dc.v3ToStr(self.gripcontactnormals[i][0]), dc.v3ToStr(self.gripcontactnormals[i][1]),
dc.mat4ToStr(self.griprotmats[i]), str(self.gripjawwidth[i]), idhand)
gdb.execute(sql)
else:
print "Grasps already saved or duplicated filename!"
def saveToDB(self, gdb):
"""
save the result to mysqldatabase
:param gdb: is an object of the GraspDB class in the database package
:return:
author: weiwei
date: 20170110
"""
# save to database
gdb = db.GraspDB()
idhand = gdb.loadIdHand(self.handname)
idobject = gdb.loadIdObject(self.dbobjname)
sql = "SELECT * FROM freeairgrip, object WHERE freeairgrip.idobject LIKE '%s' AND \
freeairgrip.idhand LIKE '%s'" % (idobject, idhand)
result = gdb.execute(sql)
if len(result) > 0:
print "Grasps already saved or duplicated filename!"
isredo = raw_input("Do you want to overwrite the database? (Y/N)")
if isredo != "Y" and isredo != "y":
print "Grasp planning aborted."
else:
sql = "DELETE FROM freeairgrip WHERE freeairgrip.idobject LIKE '%s' AND \
freeairgrip.idhand LIKE '%s'" % (idobject, idhand)
gdb.execute(sql)
print self.gripcontacts
for i in range(len(self.gripcontacts)):
sql = "INSERT INTO freeairgrip(idobject, contactpnt0, contactpnt1, \
contactnormal0, contactnormal1, rotmat, jawwidth, idhand) \
VALUES('%s', '%s', '%s', '%s', '%s', '%s', '%s', %d)" % \
(idobject, dc.v3ToStr(self.gripcontacts[i][0]), dc.v3ToStr(self.gripcontacts[i][1]),
dc.v3ToStr(self.gripcontactnormals[i][0]), dc.v3ToStr(self.gripcontactnormals[i][1]),
dc.mat4ToStr(self.griprotmats[i]), str(self.gripjawwidth[i]), idhand)
gdb.execute(sql)
def 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)
if __name__=="__main__":
# show in panda3d
from direct.showbase.ShowBase import ShowBase
from panda3d.core import *
from panda3d.bullet import BulletSphereShape
import pandaplotutils.pandageom as pandageom
import pandaplotutils.pandactrl as pandactrl
from direct.filter.CommonFilters import CommonFilters
base = pandactrl.World(lookatp=[0,0,0])
handpkg = rtq85nm
# handpkg = hrp5threenm
gdb = db.GraspDB()
this_dir, this_filename = os.path.split(__file__)
objpath = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "planewheel.stl")
# objpath = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "ttube.stl")
# objpath = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "tool2.stl")
# objpath = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "sandpart.stl")
# objpath = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "planerearstay.stl")
fpose = FloatingPoses(objpath, gdb, handpkg, base)
# fpose.showIcomat4s(base.render)
# usage:
# genFPandGs generate floatingposes and grips
# saveToDB save the generated FP and Gs into database
# updateDBwithFGPairs compute the FGpairs without considering ik
# updateDBwithIK compute the ik feasible FGs
this_dir, this_filename = os.path.split(__file__)
objpath = os.path.join(this_dir, "grip/objects", "bunnysim.stl")
objnp = base.pg.loadstlaspandanp_fn(objpath)
objnp.setColor(.5, 0, 0, 1)
objnp.reparentTo(base.render)
tblpath = os.path.join(this_dir, "env", "table.stl")
tblnp = base.pg.loadstlaspandanp_fn(tblpath)
tblnp.setColor(.5, .5, .5, 1)
tblnp.setPos(0, 0, 0)
tblnp.reparentTo(base.render)
handpkg = rtq85nm
cdchecker = BNPchecker(base)
gdb = db.GraspDB(database="ur3dualgrip")
data = gdb.loadFreeAirGrip('bunnysim', 'rtq85')
if data:
freegripid, freegripcontacts, freegripnormals, freegriprotmats, freegripjawwidth = data
print(len(freegripid))
for i, freegriprotmat in enumerate(freegriprotmats):
# if i>12 and i <100:
rtqhnd = rtq85nm.Rtq85NM(hndcolor=[1, 1, 1, .2])
rtqhnd.setMat(pandanpmat4=freegriprotmat)
rtqhnd.setJawwidth(freegripjawwidth[i])
tic = time.time()
cdchecker.isObjObjCollided(rtqhnd.handnp, tblnp)
# cdchecker.showObjObj(rtqhnd.handnp, tblnp)
# break
toc = time.time()
print(toc - tic)
def updateODE(self, task):
simcontrol = time.time()
# limittime=5
# if simcontrol - self.simcontrolstart > limittime*60:
# print "Time out ",limittime,"mins"
# return task.done
self.odespace.collide((self.odeworld, self.contacts),
self.near_callback)
self.odeworld.quickStep(globalClock.getDt() * 1.5)
#for smiley in self.partlist:
body = self.objnodepath.getPythonTag("body")
self.objnodepath.setPosQuat(body.getPosition(),
Quat(body.getQuaternion()))
self.workcellbody.setPosition(0, 0, 0)
self.workcell.setPosQuat(self.workcellbody.getPosition(),
Quat(self.workcellbody.getQuaternion()))
joint = OdeFixedJoint(self.odeworld)
joint.attachBody(self.workcellbody, 0)
thistime = body.getPosition()
thistimerot = body.getRotation()
intervaltime = 2 # 1s
droptime = 20 # 10s
siminterval = time.time()
if siminterval - self.interval0 > intervaltime:
self.interval0 = siminterval
if siminterval - self.simdrop > droptime:
print "out of droptime,reset"
self.resetobj()
task.again
if self.checkrange(body.getPosition()):
print "out of work cell range", body.getPosition()
self.resetobj()
task.again
diff = 1.0 # difference of pos
if (self.checkDiff(thistime, self.lasttime0) < diff
): # and self.checkDiffRot(thistimerot, self.lasttime0):
self.rotmat = body.getRotation()
self.posmat = body.getPosition()
print "get stablepos: ", self.poscount
# save rot pos to sql database
gdb = db.GraspDB()
#check=self.checkDBrepeat(gdb)
check = 1
# print "check",check
if check:
self.saveToDB(gdb)
# else:
# self.checkcount=self.checkcount+1
# if self.checkcount>=3:
# print "repeat 3 times,stop dropping"
# return task.done
# reset the environment
self.poscount = self.poscount + 1
# if task num haven't all done
if self.ntimes >= self.poscount:
self.resetobj()
task.again
else:
print "Done Update"
return task.done
self.lasttime0 = thistime
self.contacts.empty()
return task.cont
hmax=1.0)
freegriptst.segShow(base,
togglesamples=False,
togglenormals=False,
togglesamples_ref=False,
togglenormals_ref=False,
togglesamples_refcls=False,
togglenormals_refcls=False)
# freegriptst.removeFgrpcc(base)
# freegriptst.removeHndcc(base, discretesize = 16)
freegriptst.planGrasps(base, discretesize=8)
# use wrs database for ur3d
gdb = db.GraspDB(database="wrs")
# use nxt database for nextage!
# gdb = db.GraspDB(database="nxt")
freegriptst.saveToDB(gdb)
#
# def updateshow(task):
# # freegriptst.removeFgrpccShow(base)
# # freegriptst.removeFgrpccShowLeft(base)
# freegriptst.removeHndccShow(base)
# # # print task.delayTime
# # # if abs(task.delayTime-13) < 1:
# # # task.delayTime -= 12.85
# return task.again
#
# taskMgr.doMethodLater(.1, updateshow, "tickTask")
# taskMgr.add(updateshow, "tickTask")
-newcv[1],
-newcv[2],
-newav[0],
-newav[1],
newav[2],
jawwidth=17))
# newyihand.setColor(.7,.7,.7,1)
newyihand.reparentTo(base.render)
# yihnd = hndfa.genHand(usesuction=True)
# c0nvec = np.array([0, -1, 0])
# approachvec = np.array([1, 0, 0])
# for z in [60, 90]:
# for anglei in [0, 22.5, 45, 67.5, 90, 112.5, 135, 157.5, 180, 202.5, 225, 247.5, 270, 292.5, 315, 337.5]:
# newcv = np.dot(rm.rodrigues((0, 0, 1), anglei), c0nvec)
# tempav = np.dot(rm.rodrigues((0, 0, 1), anglei), approachvec)
# for anglej in [0, 22.5, 45, 67.5, 90, 112.5, 135, 157.5, 180, 202.5, 225, 247.5, 270, 292.5, 315, 337.5]:
# newyihand = yihnd.copy()
# newav = np.dot(rm.rodrigues(newcv, anglej), tempav)
# base.pggen.plotAxis(newyihand.hndnp, length=15, thickness=2)
# pregrasps.append(
# newyihand.approachAt(0, 0, z, newcv[0], newcv[1], newcv[2], newav[0], newav[1],
# newav[2], jaw_width=17))
# # toggle this part for manually defined plans
fgplanner = freegrip.Freegrip(objpath, yihnd)
gdb = db.GraspDB(database="yumi")
print("Saving to database...")
fgplanner.saveManuallyDefinedToDB(gdb, pregrasps)
base.run()
base = pandactrl.World(camp=[700,300,700], lookatp=[0,0,0])
this_dir, this_filename = os.path.split(__file__)
# objpath = os.path.join(this_dir, "objects", "housing.stl")
# objpath = os.path.join(this_dir, "objects", "box.stl")
# objpath = os.path.join(this_dir, "objects", "tool_motordriver.stl")
# objpath = os.path.join(this_dir, "objects", "bunnysim.stl")
# objpath = os.path.join(this_dir, "objects", "new_LSHAPE.stl")
# objpath = os.path.join(this_dir, "objects", "candy.STL")
objpath = os.path.join(this_dir, "objects", "twostairpeg_handle.STL")
# handpkg = rtq85nm
handpkg = sck918
# use wrs database for ur3d
# gdb = db.GraspDB(database="wrs")
# use nxt database for nextage!
gdb = db.GraspDB(database="nxt")
tps = fttp.FreeTabletopPlacement(objpath, handpkg, gdb)
# # plot obj and its convexhull
# geom = pandageom.packpandageom(tps.objtrimesh.vertices,
# tps.objtrimesh.face_normals,
# tps.objtrimesh.faces)
# node = GeomNode('obj')
# node.addGeom(geom)
# star = NodePath('obj')
# star.attachNewNode(node)
# star.setColor(Vec4(1,1,0,1))
# star.setTransparency(TransparencyAttrib.MAlpha)
# star.reparentTo(base.render)
# geom = pandageom.packpandageom(tps.objtrimeshconv.vertices,
# tps.objtrimeshconv.face_normals,
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)