def update(rbtmnp, motioncounter, rbt, path, armname, rbtmg, objcm, task):
global obj_ee_t
if motioncounter[0] < len(path):
if rbtmnp[0] is not None:
rbtmnp[0].detachNode()
pose = path[motioncounter[0]]
rbt.movearmfk(pose, armname)
rbtmnp[0] = rbtmg.genmnp(rbt, 0, obj.height)
pos = rbt.lftarm[-1]['linkend']
rot = rbt.lftarm[-1]['rotmat']
ee_w_t[:3, :3] = rot
ee_w_t[:3, 3] = pos
obj_w_t = np.dot(ee_w_t, obj_ee_t)
objcm.setMat(pg.npToMat4(obj_w_t[:3, :3]))
objcm.setPos(obj_w_t[0, 3], obj_w_t[1, 3], obj_w_t[2, 3])
objcm.reparentTo(base.render)
rbtmnp[0].reparentTo(base.render)
motioncounter[0] += 1
else:
motioncounter[0] = 0
return task.again
hrp5nplot.plotstick(base.render, hrp5nrobot)
handpkg = hrp5threenm
handpkg = rtq85nm
hrp5nmeshgen = hrp5nmesh.Hrp5NMesh(handpkg)
hrp5nmnp = hrp5nmeshgen.genmnp(hrp5nrobot)
hrp5nmnp.reparentTo(base.render)
objpos = np.array([500, -400, 305])
objrot = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]])
objrot = np.array([[0.125178158283, 0.00399381108582, 0.992126166821],
[0.98617619276, -0.109927728772, -0.123984932899],
[0.108567006886, 0.993931531906, -0.0176991540939]]).T
objrot = np.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]])
objrotmat4 = pg.npToMat4(objrot)
objrotmat4 = objrotmat4 * Mat4.rotateMat(30, Vec3(1, 0, 0))
objrot = pg.mat3ToNp(objrotmat4.getUpper3())
objpos = np.array([401.67913818, -644.12841797, 0])
objrot = np.array([[1.93558640e-06, -8.36298645e-01, 5.48274219e-01],
[1.93560686e-06, -5.48274219e-01, -8.36298645e-01],
[1.00000000e+00, 2.67997166e-06, 5.57513317e-07]])
# lfthnd
# objpos = np.array([180,130,100])
# objrot = np.array([[0,0,-1],[1,0,0],[0,-1,0]])
armid = "rgt"
armjntsgoal = hrp5nrobot.numikr(objpos, objrot, armid)
if armjntsgoal is not None:
hrp5nrobot.movearmfkr(armjntsgoal, armid)
hrp5nmnp = hrp5nmeshgen.genmnp(hrp5nrobot)
# hrp5nmnp.reparentTo(base.render)
ur5mnp = ur5dualplot.genmnp(ur5dualrobot, handpkg)
ur5mnp.reparentTo(base.render)
# goal hand
# from manipulation.grip.robotiq85 import rtq85nm
# hrp5robotrgthnd = rtq85nm.Rtq85NM()
# hrp5robotrgthnd.setColor([1,0,0,.3])
# hrp5robotrgtarmlj9_rotmat = pandageom.cvtMat4(objrot, objpos+objrot[:,0]*130)
# pg.plotAxisSelf(base.render, objpos, hrp5robotrgtarmlj9_rotmat)
# hrp5robotrgthnd.setMat(hrp5robotrgtarmlj9_rotmat)
# hrp5robotrgthnd.reparentTo(base.render)
#
# angle = nxtik.eurgtbik(objpos)
# nxtrobot.movewaist(angle)
# armjntsgoal=nxtik.numik(nxtrobot, objpos, objrot)
#
# # nxtplot.plotstick(base.render, nxtrobot)
# pandamat4=Mat4()
# pandamat4.setRow(3,Vec3(0,0,250))
# # pg.plotAxis(base.render, pandamat4)
# # nxtplot.plotmesh(base, nxtrobot)
# # pandageom.plotAxis(base.render, pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos']))
# pg.plotDumbbell(base.render, objpos, objpos, rgba = [1,0,0,1])
pg.plotAxisSelf(base.render, objpos, pg.npToMat4(objrot))
# pg.plotArrow(base.render, hrp5robot.rgtarm[8]['linkpos'], hrp5robot.rgtarm[8]['linkpos']+hrp5robot.rgtarm[8]['rotax']*1000)
#
# # nxtrobot.movearmfk6(armjntsgoal)
# # nxtmnp = nxtplot.genNxtmnp_nm(nxtrobot, plotcolor=[1,0,0,1])
# # nxtmnp.reparentTo(base.render)
base.run()
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 genmnp(self, robot, jawwidthrgt = 0, jawwidthlft = 0,
toggleendcoord = True, togglejntscoord = False, name = 'robotmesh'):
"""
generate the mesh model of ur5
mnp means mesh nodepath
:param robotjoints: the joint positions of ur5
:param toggleendcoord: whether to plot end coordinate systems
:param togglejntscoord: whether to plot joints coordinate systems
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20180130
"""
robotmesh_nodepath = NodePath(name)
# robotmesh_nodepath.setMat(pg.npToMat4(robot.base['rotmat'], robot.base['linkpos']))
robotwaist_rotmat = pg.npToMat4(robot.base['rotmat'], npvec3=robot.base['linkpos'])
self.__robotwaist_nodepath.setMat(robotwaist_rotmat)
# rgt
# base
ur3rgtbase_rotmat = pg.npToMat4(robot.rgtarm[1]['rotmat'], robot.rgtarm[1]['linkpos'])
self.__ur3rgtbase_nodepath.setMat(ur3rgtbase_rotmat)
self.__ur3rgtbase_nodepath.setColor(.5,.5,.5,1)
# shoulder
ur3rgtshoulder_rotmat = pg.npToMat4(robot.rgtarm[1]['rotmat'], robot.rgtarm[1]['linkpos'])
self.__ur3rgtshoulder_nodepath.setMat(ur3rgtshoulder_rotmat)
self.__ur3rgtshoulder_nodepath.setColor(.1,.3,.5,1)
# upperarm
ur3rgtupperarm_rotmat = pg.npToMat4(robot.rgtarm[2]['rotmat'], robot.rgtarm[2]['linkpos'])
self.__ur3rgtupperarm_nodepath.setMat(ur3rgtupperarm_rotmat)
self.__ur3rgtupperarm_nodepath.setColor(.7,.7,.7,1)
# forearm
ur3rgtforearm_rotmat = pg.npToMat4(robot.rgtarm[3]['rotmat'], robot.rgtarm[3]['linkpos'])
self.__ur3rgtforearm_nodepath.setMat(ur3rgtforearm_rotmat)
self.__ur3rgtforearm_nodepath.setColor(.35,.35,.35,1)
# wrist1
ur3rgtwrist1_rotmat = pg.npToMat4(robot.rgtarm[4]['rotmat'], robot.rgtarm[4]['linkpos'])
self.__ur3rgtwrist1_nodepath.setMat(ur3rgtwrist1_rotmat)
self.__ur3rgtwrist1_nodepath.setColor(.7,.7,.7,1)
# wrist2
ur3rgtwrist2_rotmat = pg.npToMat4(robot.rgtarm[5]['rotmat'], robot.rgtarm[5]['linkpos'])
self.__ur3rgtwrist2_nodepath.setMat(ur3rgtwrist2_rotmat)
self.__ur3rgtwrist2_nodepath.setColor(.1,.3,.5,1)
# wrist3
ur3rgtwrist3_rotmat = pg.npToMat4(robot.rgtarm[6]['rotmat'], robot.rgtarm[6]['linkpos'])
self.__ur3rgtwrist3_nodepath.setMat(ur3rgtwrist3_rotmat)
self.__ur3rgtwrist3_nodepath.setColor(.5,.5,.5,1)
self.__robotwaist_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtbase_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtshoulder_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtupperarm_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtforearm_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtwrist1_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtwrist2_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtwrist3_nodepath.reparentTo(robotmesh_nodepath)
# endcoord
if toggleendcoord:
self.pggen.plotAxis(robotmesh_nodepath,
spos = robot.rgtarm[-1]['linkend'],
pandamat3 = pg.npToMat3(robot.rgtarm[-1]['rotmat']))
# toggle all coord
if togglejntscoord:
for i in range(1, 7):
self.pggen.plotAxis(robotmesh_nodepath, spos=robot.rgtarm[i]['linkpos'],
pandamat3=pg.npToMat3(robot.rgtarm[i]['refcs']))
# hand
if self.rgthnd is not None:
self.rgthnd.setMat(pg.npToMat4(robot.rgtarm[6]['rotmat'], robot.rgtarm[6]['linkpos']))
self.rgthnd.setJawwidth(jawwidthrgt)
self.rgthnd.reparentTo(robotmesh_nodepath)
#lft
# base
ur3lftbase_rotmat = pg.npToMat4(robot.lftarm[1]['rotmat'], robot.lftarm[1]['linkpos'])
self.__ur3lftbase_nodepath.setMat(ur3lftbase_rotmat)
self.__ur3lftbase_nodepath.setColor(.5,.5,.5,1)
# shoulder
ur3lftshoulder_rotmat = pg.npToMat4(robot.lftarm[1]['rotmat'], robot.lftarm[1]['linkpos'])
self.__ur3lftshoulder_nodepath.setMat(ur3lftshoulder_rotmat)
self.__ur3lftshoulder_nodepath.setColor(.1,.3,.5,1)
# upperarm
ur3lftupperarm_rotmat = pg.npToMat4(robot.lftarm[2]['rotmat'], robot.lftarm[2]['linkpos'])
self.__ur3lftupperarm_nodepath.setMat(ur3lftupperarm_rotmat)
self.__ur3lftupperarm_nodepath.setColor(.7,.7,.7,1)
# forearm
ur3lftforearm_rotmat = pg.npToMat4(robot.lftarm[3]['rotmat'], robot.lftarm[3]['linkpos'])
self.__ur3lftforearm_nodepath.setMat(ur3lftforearm_rotmat)
self.__ur3lftforearm_nodepath.setColor(.35,.35,.35,1)
# wrist1
ur3lftwrist1_rotmat = pg.npToMat4(robot.lftarm[4]['rotmat'], robot.lftarm[4]['linkpos'])
self.__ur3lftwrist1_nodepath.setMat(ur3lftwrist1_rotmat)
self.__ur3lftwrist1_nodepath.setColor(.7,.7,.7,1)
# wrist2
ur3lftwrist2_rotmat = pg.npToMat4(robot.lftarm[5]['rotmat'], robot.lftarm[5]['linkpos'])
self.__ur3lftwrist2_nodepath.setMat(ur3lftwrist2_rotmat)
self.__ur3lftwrist2_nodepath.setColor(.1,.3,.5,1)
# wrist3
ur3lftwrist3_rotmat = pg.npToMat4(robot.lftarm[6]['rotmat'], robot.lftarm[6]['linkpos'])
self.__ur3lftwrist3_nodepath.setMat(ur3lftwrist3_rotmat)
self.__ur3lftwrist3_nodepath.setColor(.5,.5,.5,1)
self.__ur3lftbase_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3lftshoulder_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3lftupperarm_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3lftforearm_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3lftwrist1_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3lftwrist2_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3lftwrist3_nodepath.reparentTo(robotmesh_nodepath)
# endcoord
if toggleendcoord:
self.pggen.plotAxis(robotmesh_nodepath,
spos = robot.lftarm[-1]['linkend'],
pandamat3 = pg.npToMat3(robot.lftarm[-1]['rotmat']))
# toggle all coord
if togglejntscoord:
for i in range(1, 7):
self.pggen.plotAxis(robotmesh_nodepath, spos = robot.lftarm[i]['linkpos'],
pandamat3 = pg.npToMat3(robot.lftarm[i]['refcs']))
# hand
if self.lfthnd is not None:
self.lfthnd.setMat(pg.npToMat4(robot.lftarm[6]['rotmat'], robot.lftarm[6]['linkpos']))
self.lfthnd.setJawwidth(jawwidthlft)
self.lfthnd.reparentTo(robotmesh_nodepath)
return copy.deepcopy(robotmesh_nodepath)
def __init__(self, jawwidth=85, hndcolor=None, ftsensoroffset = 52):
'''
load the robotiq85 model, set jawwidth and return a nodepath
the rtq85 gripper is composed of a parallelism and a fixed triangle,
the parallelism: 1.905-1.905; 5.715-5.715; 70/110 degree
the triangle: 4.75 (finger) 5.715 (inner knuckle) 3.175 (outer knuckle)
NOTE: the setColor function is only useful when the models dont have any materials
## input
pandabase:
the showbase() object
jawwidth:
the distance between fingertips
ftsensoroffset:
the offset for ftsensor
## output
rtq85np:
the nodepath of this rtq85 hand
author: weiwei
date: 20160627
'''
self.rtq85np = NodePath("rtq85hnd")
self.jawwidth = jawwidth
self.__ftsensoroffset = ftsensoroffset
this_dir, this_filename = os.path.split(__file__)
rtq85basepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_base_link_nm.egg"))
rtq85fingerpath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_finger_link_nm.egg"))
rtq85fingertippath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_finger_tip_link_nm.egg"))
rtq85innerknucklepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_inner_knuckle_link_nm.egg"))
rtq85knucklepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_knuckle_link_nm.egg"))
rtq85base = NodePath("rtq85base")
rtq85lknuckle = NodePath("rtq85lknuckle")
rtq85rknuckle = NodePath("rtq85rknuckle")
rtq85lfgr = NodePath("rtq85lfgr")
rtq85rfgr = NodePath("rtq85rfgr")
rtq85ilknuckle = NodePath("rtq85ilknuckle")
rtq85irknuckle = NodePath("rtq85irknuckle")
rtq85lfgrtip = NodePath("rtq85lfgrtip")
rtq85rfgrtip = NodePath("rtq85rfgrtip")
# loader is a global variable defined by panda3d
rtq85_basel = loader.loadModel(rtq85basepath)
rtq85_fingerl = loader.loadModel(rtq85fingerpath)
rtq85_fingertipl = loader.loadModel(rtq85fingertippath)
rtq85_innerknucklel = loader.loadModel(rtq85innerknucklepath)
rtq85_knucklel = loader.loadModel(rtq85knucklepath)
# base
rtq85_basel.instanceTo(rtq85base)
if hndcolor is None:
# rtq85base.setColor(.2,.2,.2,1)
pass
else:
rtq85base.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85base.setTransparency(TransparencyAttrib.MAlpha)
# left and right outer knuckle
rtq85_knucklel.instanceTo(rtq85lknuckle)
rtq85lknuckle.setPos(-30.60114443, 54.90451627, 0)
rtq85lknuckle.setHpr(0, 0, 180)
if hndcolor is None:
# rtq85lknuckle.setColor(.5,.5,.5,1)
pass
else:
rtq85lknuckle.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85lknuckle.setTransparency(TransparencyAttrib.MAlpha)
rtq85lknuckle.reparentTo(rtq85base)
rtq85_knucklel.instanceTo(rtq85rknuckle)
rtq85rknuckle.setPos(30.60114443, 54.90451627, 0)
rtq85rknuckle.setHpr(0, 0, 0)
if hndcolor is None:
# rtq85rknuckle.setColor(.5,.5,.5,1)
pass
else:
rtq85rknuckle.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85rknuckle.setTransparency(TransparencyAttrib.MAlpha)
rtq85rknuckle.reparentTo(rtq85base)
# left and right finger
rtq85_fingerl.instanceTo(rtq85lfgr)
rtq85lfgr.setPos(31.48504435, -4.08552455, 0)
if hndcolor is None:
# rtq85lfgr.setColor(0.2,0.2,0.2,1)
pass
else:
rtq85lfgr.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85lfgr.setTransparency(TransparencyAttrib.MAlpha)
rtq85lfgr.reparentTo(rtq85lknuckle)
rtq85_fingerl.instanceTo(rtq85rfgr)
rtq85rfgr.setPos(31.48504435, -4.08552455, 0)
if hndcolor is None:
# rtq85rfgr.setColor(0.2,0.2,0.2,1)
pass
else:
rtq85rfgr.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85rfgr.setTransparency(TransparencyAttrib.MAlpha)
rtq85rfgr.reparentTo(rtq85rknuckle)
# left and right inner knuckle
rtq85_innerknucklel.instanceTo(rtq85ilknuckle)
rtq85ilknuckle.setPos(-12.7, 61.42, 0)
rtq85ilknuckle.setHpr(0, 0, 180)
if hndcolor is None:
# rtq85ilknuckle.setColor(.5,.5,.5,1)
pass
else:
rtq85ilknuckle.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85ilknuckle.setTransparency(TransparencyAttrib.MAlpha)
rtq85ilknuckle.reparentTo(rtq85base)
rtq85_innerknucklel.instanceTo(rtq85irknuckle)
rtq85irknuckle.setPos(12.7, 61.42, 0)
rtq85irknuckle.setHpr(0, 0, 0)
if hndcolor is None:
# rtq85irknuckle.setColor(.5,.5,.5,1)
pass
else:
rtq85irknuckle.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85irknuckle.setTransparency(TransparencyAttrib.MAlpha)
rtq85irknuckle.reparentTo(rtq85base)
# left and right fgr tip
rtq85_fingertipl.instanceTo(rtq85lfgrtip)
rtq85lfgrtip.setPos(37.59940821, 43.03959807, 0)
if hndcolor is None:
# rtq85lfgrtip.setColor(.5,.5,.5,1)
pass
else:
rtq85lfgrtip.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85lfgrtip.setTransparency(TransparencyAttrib.MAlpha)
rtq85lfgrtip.reparentTo(rtq85ilknuckle)
rtq85_fingertipl.instanceTo(rtq85rfgrtip)
rtq85rfgrtip.setPos(37.59940821, 43.03959807, 0)
rtq85rfgrtip.setHpr(0, 0, 0)
if hndcolor is None:
# rtq85rfgrtip.setColor(.5,.5,.5,1)
pass
else:
rtq85rfgrtip.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85rfgrtip.setTransparency(TransparencyAttrib.MAlpha)
rtq85rfgrtip.reparentTo(rtq85irknuckle)
# rotate to x, y, z coordinates (this one rotates the base, not the self.rtq85np)
rtq85base.setMat(pandageom.npToMat4(rm.rodrigues([1,0,0], 90))*rtq85base.getMat())
rtq85base.setPos(0,0,self.__ftsensoroffset)
rtq85base.reparentTo(self.rtq85np)
self.setJawwidth(jawwidth)
self.__jawwidthopen = 85.0
self.__jawwidthclosed = 0.0
def match(self, perceivedpnts, perceivednormals, ddist=5.0, dangle=30.0):
"""
do a match
:return: rotmats of top matches
author: weiwei
date: 20170802
"""
# save txt
pverts = np.array([tuple(x) for x in perceivedpnts],
dtype=[('x', 'f4'), ('y', 'f4'), ('z', 'f4')])
el = PlyElement.describe(pverts, 'vertex')
PlyData([el], text=True).write('perceivedpnts.ply')
# save txt
vandnarray = []
for i in range(len(self.temppnts)):
v = self.temppnts[i]
n = self.tempnormals[i]
vandn = (v[0], v[1], v[2], n[0], n[1], n[2])
vandnarray.append(vandn)
pverts = np.array(vandnarray, dtype=[('x', 'f4'),('y', 'f4'),('z', 'f4'),\
('nx','f4'),('ny','f4'),('nz','f4')])
el = PlyElement.describe(pverts, 'vertex')
PlyData([el], text=True).write('ttube.ply')
accspace = {}
# get the preceived global model descriptor
nperceivedpnts = perceivedpnts.shape[0]
i = np.argmax(perceivedpnts, axis=0)[2]
for j in range(0, nperceivedpnts):
print j, nperceivedpnts
m_0 = np.asarray(perceivedpnts[i])
m_1 = np.asarray(perceivedpnts[j])
v_m0m1 = m_0 - m_1
v_m1m0 = m_1 - m_0
n_m0 = perceivednormals[i]
n_m1 = perceivednormals[j]
# f1, namely ||d||2
f1 = np.linalg.norm(m_0 - m_1)
# f2, namely angle between n_m0 and v_m1m0
f2 = rm.degree_between(n_m0, v_m1m0)
# f3, namely angle between n_m1 and v_m0m1
f3 = rm.degree_between(n_m1, v_m0m1)
# f4, namely angle between n_m0 and n_m1
f4 = rm.degree_between(n_m0, n_m1)
# discretize the values
try:
f1d = int(math.floor(f1 / ddist) * ddist + ddist)
f2d = int(math.floor(f2 / dangle) * dangle + dangle)
f3d = int(math.floor(f3 / dangle) * dangle + dangle)
f4d = int(math.floor(f4 / dangle) * dangle + dangle)
except:
continue
key = (f1d, f2d, f3d, f4d)
# angle between n_m0 and x+
xplus = np.asarray([1, 0, 0])
yplus = np.asarray([0, 1, 0])
nm0xangle = math.degrees(rm.radian_between(n_m0, xplus))
rotax = np.cross(xplus, n_m0)
if np.isnan(rotax).any() or not rotax.any():
continue
rotmat = rm.rodrigues(rotax, nm0xangle)
v_m1m0onxplus = np.dot(v_m1m0, rotmat)
v_m1m0onxplusyzproj = np.asarray(
[0, v_m1m0onxplus[1], v_m1m0onxplus[2]])
alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus)
if v_m1m0onxplus[2] < 0:
alpha_m0 = 2 * math.pi - alpha_m0
if key in self.gmd.keys():
malist = self.gmd[key]
print len(malist)
for maslot in malist:
alpha = math.degrees(alpha_m0 - maslot[2])
try:
alphadiscrete = int(
math.floor(alpha / dangle) * dangle + dangle)
except:
continue
acckey = (maslot[0], alphadiscrete)
if acckey in accspace.keys():
accspace[acckey] += 1
else:
accspace[acckey] = 1
if len(accspace.keys()) is 0:
return (None, None)
# find top matches and rot matrices
maxn = sorted(accspace.iteritems(),
key=operator.itemgetter(1),
reverse=True)[:5]
rotmat4list = []
silist = []
milist = []
for maxnele in maxn:
mi, alpha = maxnele[0]
# step1 move to temppnts[mi]
displacement0 = -self.temppnts[mi]
rotmat4_0 = Mat4.translateMat(displacement0[0], displacement0[1],
displacement0[2])
# step2 rotate to goal
normalangle = math.degrees(
rm.radian_between(self.tempnormals[mi], perceivednormals[i]))
normalrotax = np.cross(self.tempnormals[mi], perceivednormals[i])
normalrotmat = rm.rodrigues(normalrotax, normalangle)
anglerotmat = rm.rodrigues(perceivednormals[i], -alpha)
rotmat = np.dot(anglerotmat, normalrotmat)
rotmat4_1 = pg.npToMat4(rotmat)
# step3 move to perceivedpnts[i]
displacement1 = perceivedpnts[i]
rotmat4_2 = Mat4.translateMat(displacement1[0], displacement1[1],
displacement1[2])
rotmat4 = rotmat4_0 * rotmat4_1 * rotmat4_2
rotmat4list.append(rotmat4)
silist.append(i)
milist.append(mi)
return rotmat4list, silist, milist
def gencm(self, startposobj, startrotobj, startposrbt, startrotrbt, isrotated = False, iscornered = False, isdrooped = False, isinclined = False):
"""
make the collision model of the object, set its pose according to the robot end effector pose
:param startposobj: object position
:param startrotobj: object orientation
:param startposrbt: robot position
:param startrotrbt: robot orientation
:param isrotated: is the object to be rotated 90 degrees about the vertical axis of the world frame
:param iscornered:is the object grasped from its corner
:param isdrooped: is the object drooped due to orientation
:param isinclined: is the object at an inclined pose
:return: collision model of the object with set pose
"""
#load the object and set its pose related to the robot pose
startposobj = startposobj
startrotobj = startrotobj
startpos = startposrbt
startrot = startrotrbt
#create the collision model
self.objcm = cm.CollisionModel(objinit=self.objpath)
# if required to flip the object and the robot pose
# startrot = rot_transform(startrot, 180)
# set the object pose in hand
# easy to be automated for planning
if isrotated:
self.torque = 9.81 * self.m * self.width / 2 / 1000 # Nm
print("rotated torque is set!")
print("self.torque is:", self.torque)
rotz = rm.rodrigues([0, 0, 1], -90)
tmp_rot = np.dot(rotz, startrotobj)
startrotobj = np.dot(tmp_rot, startrotobj)
startposobj[0] -= (self.length/2-self.width/2)
if isinclined:
incline_angle = 10
tmp_incline_rot = rm.rodrigues(startrot[:,2],incline_angle)
startrot = np.dot(tmp_incline_rot,startrot)
startrotobj = np.dot(tmp_incline_rot, startrotobj)
objcmcopy = copy.deepcopy(self.objcm)
objcmcopy.setMat(pg.npToMat4(startrotobj,startposobj))
##for visualization of the limit
# for incangle in range(0,20,10):
# print (incangle)
# tmp_incline_rot = rm.rodrigues(startrot[:, 2], incangle)
# startrotobjinc = np.dot(tmp_incline_rot, startrotobj)
# objcmcopy = copy.deepcopy(objcm)
# startrotrbt = np.dot(tmp_incline_rot, startrot)
# rbtangles = rbt.numik(startpos, startrotrbt, "lft")
# rbt.movearmfk(rbtangles, "lft")
# rbtmg.genmnp(rbt, jawwidthlft=objheight,toggleendcoord=False).reparentTo(base.render)
# objcmcopy.setMat(pg.npToMat4(startrotobjinc, startposobj))
# if incangle == 10:
# objcmcopy.setColor(.8, .0, .0, .5)
# objcmcopy.reparentTo(base.render)
# base.run()
if iscornered:
corner_angle = 45
rotz = rm.rodrigues([0,0,1],corner_angle)
tmp_rot = np.dot(rotz,startrotobj)
startrotobj = np.dot(tmp_rot,startrotobj)
grasppnt = [-self.length/2, self.width/2,0,1]
obj_w_t = np.eye(4)
obj_w_t[:3,:3] = startrotobj
obj_w_t[:3,3] = startposobj
print("obj_w_t",obj_w_t)
grasppnt_w = np.dot(obj_w_t,grasppnt)
print(grasppnt_w)
print(startpos)
pos_diff = [a-b for a,b in zip(grasppnt_w,startpos)]
print ("Position difference is: ", pos_diff)
print("Position difference is: ", [grasppnt_w[0],grasppnt_w[1],grasppnt_w[2]]-startpos)
startposobj-=pos_diff
if isdrooped == True:
droop_angle = -20
tmp_rot_ee_x = rm.rodrigues(startrot[:,0],droop_angle)
startrotobj = np.dot(tmp_rot_ee_x,startrotobj)
# grasppnt = [-objlength/2*np.cos(droop_angle), objwidth/2*np.sin(droop_angle), 0, 1]
# obj_w_t = np.eye(4)
# obj_w_t[:3, :3] = startrotobj
# obj_w_t[:3, 3] = startposobj
# grasppnt_w = np.dot(obj_w_t, grasppnt)
# print(grasppnt_w)
# print(startpos)
# pos_diff = [a - b for a, b in zip(grasppnt_w, startpos)]
# print("Position difference is: ", pos_diff)
# print("Position difference is: ", [grasppnt_w[0], grasppnt_w[1], grasppnt_w[2]] - startpos)
# startposobj += pos_diff
self.objcm.setMat(pg.npToMat4(startrotobj,startposobj))
return self.objcm
def __init__(self, jawwidth=50, hndcolor=None, ftsensoroffset=52):
"""
load the robotiq85 model, set jawwidth and return a nodepath
## input
pandabase:
the showbase() object
jawwidth:
the distance between fingertips
ftsensoroffset:
the offset for ftsensor
## output
sck918np:
the nodepath of this rtq85 hand
author: wangyan
date: 20190413
"""
self.sck918np = NodePath("sck918hnd")
self.handnp = self.sck918np
self.__ftsensoroffset = ftsensoroffset
self.jawwidth = jawwidth
this_dir, this_filename = os.path.split(__file__)
sck918basepath = Filename.fromOsSpecific(
os.path.join(this_dir, "schunk918egg", "sck918_base.egg"))
sck918sliderpath = Filename.fromOsSpecific(
os.path.join(this_dir, "schunk918egg", "sck918_slider.egg"))
sck918gripperpath = Filename.fromOsSpecific(
os.path.join(this_dir, "schunk918egg", "sck918_gripper.egg"))
sck918base = NodePath("sck918base")
sck918lslider = NodePath("sck918lslider")
sck918rslider = NodePath("sck918rslider")
sck918lgripper = NodePath("sck918lgripper")
sck918rgripper = NodePath("sck918rgripper")
# loader is a global variable defined by panda3d
sck918_basel = loader.loadModel(sck918basepath)
sck918_sliderl = loader.loadModel(sck918sliderpath)
sck918_gripperl = loader.loadModel(sck918gripperpath)
# base
sck918_basel.instanceTo(sck918base)
if hndcolor is None:
# rtq85base.setColor(.2,.2,.2,1)
pass
else:
sck918base.setColor(hndcolor[0], hndcolor[1], hndcolor[2],
hndcolor[3])
sck918base.setTransparency(TransparencyAttrib.MAlpha)
# sck918base.setColor(.2, .2, .2, 1.0)
sck918base.setHpr(90, -90, 0)
# left and right outer knuckle
sck918_sliderl.instanceTo(sck918lslider)
sck918lslider.setColor(.1, .1, .1, 1.0)
sck918lslider.setPos(-10, 40, 73)
sck918lslider.setHpr(-90, 0, 0)
sck918lslider.reparentTo(sck918base)
sck918_sliderl.instanceTo(sck918rslider)
sck918rslider.setColor(.1, .1, .1, 1.0)
sck918rslider.setPos(10, -40, 73)
sck918rslider.setHpr(90, 0, 0)
sck918rslider.reparentTo(sck918base)
# left and right finger
sck918_gripperl.instanceTo(sck918lgripper)
sck918lgripper.setColor(.7, .7, .7, 1.0)
sck918lgripper.setPos(-8, 20, 0)
sck918lgripper.setHpr(0, 180, 0)
sck918lgripper.reparentTo(sck918lslider)
sck918_gripperl.instanceTo(sck918rgripper)
sck918rgripper.setColor(.7, .7, .7, 1.0)
sck918rgripper.setPos(-8, 20, 0)
sck918rgripper.setHpr(0, 180, 0)
sck918rgripper.reparentTo(sck918rslider)
# rotate to x, y, z coordinates (this one rotates the base, not the self.rtq85np)
# the default x direction is facing the ee, the default z direction is facing downward
# execute this file to see the default pose
sck918base.setTransparency(TransparencyAttrib.MAlpha)
sck918base.setMat(
pandageom.npToMat4(rm.rodrigues([0, 0, 1], 90)) *
pandageom.npToMat4(rm.rodrigues([1, 0, 0], 90)) *
sck918base.getMat())
sck918base.setPos(0, 0, self.__ftsensoroffset)
sck918base.reparentTo(self.sck918np)
self.setJawwidth(jawwidth)
self.__jawwidthopen = 50.0
self.__jawwidthclosed = 0.0
def genmnp(self,
robot,
jawwidthrgt=0,
jawwidthlft=0,
toggleendcoord=False,
togglejntscoord=False,
name='robotmesh'):
"""
generate a panda3d nodepath for the robot
mnp indicates this function generates a mesh nodepath
:param robot: the robot object, see robot.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20180109
"""
robotmesh_nodepath = NodePath(name)
# body
nxtleg_rotmat = pg.npToMat4(robot.base['refcs'],
npvec3=robot.base['linkpos'])
self.__nxtleg_nodepath.setMat(nxtleg_rotmat)
nxtwaist_rotmat = pg.npToMat4(robot.base['rotmat'],
npvec3=robot.base['linkpos'] +
np.array([0.0, 0.0, 973.0]))
self.__nxtwaist_nodepath.setMat(base.render, nxtwaist_rotmat)
# rgtarm
nxtrgtarmlj0_rotmat = pg.npToMat4(robot.rgtarm[1]['rotmat'],
robot.rgtarm[1]['linkpos'])
self.__nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat)
nxtrgtarmlj1_rotmat = pg.npToMat4(robot.rgtarm[2]['rotmat'],
robot.rgtarm[2]['linkpos'])
self.__nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat)
nxtrgtarmlj2_rotmat = pg.npToMat4(robot.rgtarm[3]['rotmat'],
robot.rgtarm[3]['linkpos'])
self.__nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat)
nxtrgtarmlj3_rotmat = pg.npToMat4(robot.rgtarm[4]['rotmat'],
robot.rgtarm[4]['linkpos'])
self.__nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat)
nxtrgtarmlj4_rotmat = pg.npToMat4(robot.rgtarm[5]['rotmat'],
robot.rgtarm[5]['linkpos'])
self.__nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat)
# lftarm
nxtlftarmlj0_rotmat = pg.npToMat4(robot.lftarm[1]['rotmat'],
robot.lftarm[1]['linkpos'])
self.__nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat)
nxtlftarmlj1_rotmat = pg.npToMat4(robot.lftarm[2]['rotmat'],
robot.lftarm[2]['linkpos'])
self.__nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat)
nxtlftarmlj2_rotmat = pg.npToMat4(robot.lftarm[3]['rotmat'],
robot.lftarm[3]['linkpos'])
self.__nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat)
nxtlftarmlj3_rotmat = pg.npToMat4(robot.lftarm[4]['rotmat'],
robot.lftarm[4]['linkpos'])
self.__nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat)
nxtlftarmlj4_rotmat = pg.npToMat4(robot.lftarm[5]['rotmat'],
robot.lftarm[5]['linkpos'])
self.__nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat)
self.__nxtleg_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtrgtarmlj0_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtrgtarmlj1_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtrgtarmlj2_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtrgtarmlj3_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtrgtarmlj4_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtlftarmlj0_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtlftarmlj1_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtlftarmlj2_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtlftarmlj3_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtlftarmlj4_nodepath.reparentTo(robotmesh_nodepath)
## right
# endcoord
if toggleendcoord:
self.pggen.plotAxis(robotmesh_nodepath,
spos=robot.rgtarm[-1]['linkend'],
pandamat3=pg.npToMat3(
robot.rgtarm[-1]['rotmat']))
# toggle all coord
if togglejntscoord:
for i in range(1, 7):
self.pggen.plotAxis(robotmesh_nodepath,
spos=robot.rgtarm[i]['linkpos'],
pandamat3=pg.npToMat3(
robot.rgtarm[i]['refcs']))
# hand
if self.rgthnd is not None:
self.rgthnd.setMat(
pg.npToMat4(robot.rgtarm[-1]['rotmat'],
robot.rgtarm[-1]['linkpos']))
self.rgthnd.setJawwidth(jawwidthrgt)
self.rgthnd.reparentTo(robotmesh_nodepath)
# left
# endcoord
if toggleendcoord:
self.pggen.plotAxis(robotmesh_nodepath,
spos=robot.lftarm[-1]['linkend'],
pandamat3=pg.npToMat3(
robot.lftarm[-1]['rotmat']))
# toggle all coord
if togglejntscoord:
for i in range(1, 7):
self.pggen.plotAxis(robotmesh_nodepath,
spos=robot.lftarm[i]['linkpos'],
pandamat3=pg.npToMat3(
robot.lftarm[i]['refcs']))
# hand
if self.lfthnd is not None:
self.lfthnd.setMat(
pg.npToMat4(robot.lftarm[-1]['rotmat'],
robot.lftarm[-1]['linkpos']))
self.lfthnd.setJawwidth(jawwidthlft)
self.lfthnd.reparentTo(robotmesh_nodepath)
return copy.deepcopy(robotmesh_nodepath)
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)
ur5mnp = ur5dualplot.genmnp(ur5dualrobot, handpkg)
ur5mnp.reparentTo(base.render)
# goal hand
# from manipulation.grip.robotiq85 import rtq85nm
# hrp5robotrgthnd = rtq85nm.Rtq85NM()
# hrp5robotrgthnd.setColor([1,0,0,.3])
# hrp5robotrgtarmlj9_rotmat = pandageom.cvtMat4(objrot, objpos+objrot[:,0]*130)
# pg.plotAxisSelf(base.render, objpos, hrp5robotrgtarmlj9_rotmat)
# hrp5robotrgthnd.setMat(hrp5robotrgtarmlj9_rotmat)
# hrp5robotrgthnd.reparentTo(base.render)
#
# angle = nxtik.eurgtbik(objpos)
# nxtrobot.movewaist(angle)
# armjntsgoal=nxtik.numik(nxtrobot, objpos, objrot)
#
# # nxtplot.plotstick(base.render, nxtrobot)
# pandamat4=Mat4()
# pandamat4.setRow(3,Vec3(0,0,250))
# # pg.plotAxis(base.render, pandamat4)
# # nxtplot.plotmesh(base, nxtrobot)
# # pandageom.plotAxis(base.render, pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos']))
# pg.plotDumbbell(base.render, objpos, objpos, rgba = [1,0,0,1])
pg.plotAxisSelf(base.render, objpos, pg.npToMat4(objrot))
# pg.plotArrow(base.render, hrp5robot.rgtarm[8]['linkpos'], hrp5robot.rgtarm[8]['linkpos']+hrp5robot.rgtarm[8]['rotax']*1000)
#
# # nxtrobot.movearmfk6(armjntsgoal)
# # nxtmnp = nxtplot.genNxtmnp_nm(nxtrobot, plotcolor=[1,0,0,1])
# # nxtmnp.reparentTo(base.render)
base.run()
def saveToDB(self, positionlist, gdb, discretesize=8):
"""
: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!")
# 1) select the tabletopplacements
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 Exception("Plan the freetabletopplacement first!")
return
result = np.asarray(result)
idfreettplist = [int(x) for x in result[:, 0]]
rotmatfreettplist = [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 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)
# 3.1) check if already exist
if len(result) != 0:
print("Tabletopplacements already exist! ")
isredo = input("Do you want to overwrite the database? (Y/N)")
if isredo != "Y" and isredo != "y":
print("Placement planning aborted.")
return
else:
for idfreettp in idfreettplist:
sql = "DELETE FROM tabletopplacements WHERE \
tabletopplacements.idfreetabletopplacement LIKE '%s'" % idfreettp
gdb.execute(sql)
print("Old tabletopplacements have been deleted!")
# 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(idfreettplist, rotmatfreettplist):
for idangle, anglevalue in zip(idanglelist, anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
rotmat4 = pg.npToMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2]+";"
gdb.execute(sql)
print("Tabletop placements saved!")
# save tabletopgrips
isttg, ttgresult = self.__getTtg(gdb)
if not isttg:
idhand = gdb.loadIdHand(self.handname)
for idfree in idfreettplist:
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 availalbe grasps
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]]
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 = pg.npToMat4(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:
# TODO delete this one, placements and grasps always exist together
print("Tabletopgrips already exist!")
raise Exception("Warning! Grasps are NOT saved!")
return
print("Grasps saved!")
def match(self, perceivedpnts, perceivednormals, ddist = 5.0, dangle = 30.0):
"""
do a match
:return: rotmats of top matches
author: weiwei
date: 20170802
"""
# save txt
pverts = np.array([tuple(x) for x in perceivedpnts], dtype=[('x', 'f4'),('y', 'f4'),('z', 'f4')])
el = PlyElement.describe(pverts, 'vertex')
PlyData([el], text = True).write('perceivedpnts.ply')
# save txt
vandnarray = []
for i in range(len(self.temppnts)):
v = self.temppnts[i]
n = self.tempnormals[i]
vandn = (v[0],v[1],v[2],n[0],n[1],n[2])
vandnarray.append(vandn)
pverts = np.array(vandnarray, dtype=[('x', 'f4'),('y', 'f4'),('z', 'f4'),\
('nx','f4'),('ny','f4'),('nz','f4')])
el = PlyElement.describe(pverts, 'vertex')
PlyData([el], text = True).write('ttube.ply')
accspace = {}
# get the preceived global model descriptor
nperceivedpnts = perceivedpnts.shape[0]
i = np.argmax(perceivedpnts, axis = 0)[2]
for j in range(0,nperceivedpnts):
print j, nperceivedpnts
m_0 = np.asarray(perceivedpnts[i])
m_1 = np.asarray(perceivedpnts[j])
v_m0m1 = m_0-m_1
v_m1m0 = m_1-m_0
n_m0 = perceivednormals[i]
n_m1 = perceivednormals[j]
# f1, namely ||d||2
f1 = np.linalg.norm(m_0-m_1)
# f2, namely angle between n_m0 and v_m1m0
f2 = rm.degree_between(n_m0, v_m1m0)
# f3, namely angle between n_m1 and v_m0m1
f3 = rm.degree_between(n_m1, v_m0m1)
# f4, namely angle between n_m0 and n_m1
f4 = rm.degree_between(n_m0, n_m1)
# discretize the values
try:
f1d = int(math.floor(f1/ddist)*ddist+ddist)
f2d = int(math.floor(f2/dangle)*dangle+dangle)
f3d = int(math.floor(f3/dangle)*dangle+dangle)
f4d = int(math.floor(f4/dangle)*dangle+dangle)
except:
continue
key = (f1d, f2d, f3d, f4d)
# angle between n_m0 and x+
xplus = np.asarray([1,0,0])
yplus = np.asarray([0,1,0])
nm0xangle = math.degrees(rm.radian_between(n_m0, xplus))
rotax = np.cross(xplus, n_m0)
if np.isnan(rotax).any() or not rotax.any():
continue
rotmat = rm.rodrigues(rotax, nm0xangle)
v_m1m0onxplus = np.dot(v_m1m0, rotmat)
v_m1m0onxplusyzproj = np.asarray([0, v_m1m0onxplus[1], v_m1m0onxplus[2]])
alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus)
if v_m1m0onxplus[2] < 0:
alpha_m0 = 2*math.pi - alpha_m0
if key in self.gmd.keys():
malist = self.gmd[key]
print len(malist)
for maslot in malist:
alpha = math.degrees(alpha_m0-maslot[2])
try:
alphadiscrete = int(math.floor(alpha/dangle)*dangle+dangle)
except:
continue
acckey = (maslot[0], alphadiscrete)
if acckey in accspace.keys():
accspace[acckey] += 1
else:
accspace[acckey] = 1
if len(accspace.keys()) is 0:
return (None, None)
# find top matches and rot matrices
maxn = sorted(accspace.iteritems(), key=operator.itemgetter(1), reverse=True)[:5]
rotmat4list = []
silist = []
milist = []
for maxnele in maxn:
mi, alpha = maxnele[0]
# step1 move to temppnts[mi]
displacement0 = -self.temppnts[mi]
rotmat4_0 = Mat4.translateMat(displacement0[0], displacement0[1], displacement0[2])
# step2 rotate to goal
normalangle = math.degrees(rm.radian_between(self.tempnormals[mi], perceivednormals[i]))
normalrotax = np.cross(self.tempnormals[mi], perceivednormals[i])
normalrotmat = rm.rodrigues(normalrotax, normalangle)
anglerotmat = rm.rodrigues(perceivednormals[i], -alpha)
rotmat = np.dot(anglerotmat, normalrotmat)
rotmat4_1 = pg.npToMat4(rotmat)
# step3 move to perceivedpnts[i]
displacement1 = perceivedpnts[i]
rotmat4_2 = Mat4.translateMat(displacement1[0], displacement1[1], displacement1[2])
rotmat4 = rotmat4_0*rotmat4_1*rotmat4_2
rotmat4list.append(rotmat4)
silist.append(i)
milist.append(mi)
return rotmat4list, silist, milist