def updateshow(rbd, rbdnp, framenp, task):
for frame in framenp:
frame.detachNode()
dtime = 0.001
skipframe = 15
for i in range(skipframe):
force, torque = genForce(rbd, dtime)
[P, L] = doPhysics(rbd, force, torque, dtime)
writer.writerow([str(L[0]), str(L[1]), str(L[2])])
updateRbdPR(rbd, dtime)
if task.time > 2.0:
writer.close()
# model = loader.loadModel(model_filepath)
# model.reparentTo(base.render)
model.setMat(pg.cvtMat4(rbd.rotmat))
model.setPos(pg.npToV3(rbd.pos))
arrownp = pg.plotArrow(base.render,
epos=rbd.angularw * 500.0,
thickness=15,
rgba=[1, 0.5, 0.5, 1])
framenp.append(arrownp)
return task.again
def updateshow(ur5dualrobot, dfvalue, armid, robotnp, arrownp, task):
for robot in robotnp:
robot.detachNode()
for arrow in arrownp:
arrow.detachNode()
ur5dualmnp = u5p.genUr5dualmnp(ur5dualrobot, handpkg)
ur5dualmnp.reparentTo(base.render)
robotnp.append(ur5dualmnp)
if time.clock() > 2:
dfvalue = 0
tcpmat4 = ur5dualrobot.gettcpframe(armid)
dfvec = tcpmat4.getRow3(0) * dfvalue
arrownp.append(
pg.plotArrow(base.render,
tcpmat4.getRow3(3),
tcpmat4.getRow3(3) + dfvec * 30,
thickness=10,
rgba=[.7, .2, .2, 1]))
deltax = impx.control(dfvec[0])
deltay = impy.control(dfvec[1])
deltaz = impz.control(dfvec[2])
print deltax, deltay, deltaz
tgtpos = inittcppos + np.array([deltax, deltay, deltaz])
tgtrot = inittcprot
armjntsgoal = u5dik.numik(ur5dualrobot, tgtpos, tgtrot, armid)
if armjntsgoal is not None:
ur5dualrobot.movearmfk(armjntsgoal, armid)
return task.again
def update(task):
dt = globalClock.getDt()
world.doPhysics(dt)
vecw= topbullnode.getAngularVelocity()
arrownp = pg.plotArrow(base.render, epos = vecw*1000, thickness = 15)
# print rotmat
topbullnode.get
return task.cont
def update(task):
dt = globalClock.getDt()
world.doPhysics(dt)
vecw = topbullnode.getAngularVelocity()
arrownp = pg.plotArrow(base.render, epos=vecw * 1000, thickness=15)
# print rotmat
topbullnode.get
return task.cont
def updateshow(linknp, task):
for link in linknp:
link.detachNode()
th = sgllink.control(10000)
print th, math.degrees(th)
arrownp = pg.plotArrow(base.render, spos = np.array([0,0,0]), epos = np.array([0, math.cos(th), math.sin(th)])*linklen)
linknp.append(arrownp)
# if time.clock() > 2:
# dfvalue = 0
return task.again
def updateshow(linknp, task):
for link in linknp:
link.detachNode()
th = sgllink.control(10000)
print th, math.degrees(th)
arrownp = pg.plotArrow(
base.render,
spos=np.array([0, 0, 0]),
epos=np.array([0, math.cos(th), math.sin(th)]) * linklen)
linknp.append(arrownp)
# if time.clock() > 2:
# dfvalue = 0
return task.again
def updateshow(rbd, rbdnp, framenp, task):
for frame in framenp:
frame.detachNode()
dtime = 0.001
skipframe = 15
for i in range(skipframe):
force, torque = genForce(rbd, dtime)
[P, L] = doPhysics(rbd, force, torque, dtime)
writer.writerow([str(L[0]), str(L[1]), str(L[2])])
updateRbdPR(rbd, dtime)
if task.time > 2.0:
writer.close()
# model = loader.loadModel(model_filepath)
# model.reparentTo(base.render)
model.setMat(pg.cvtMat4(rbd.rotmat))
model.setPos(pg.npToV3(rbd.pos))
arrownp = pg.plotArrow(base.render, epos = rbd.angularw*500.0, thickness = 15, rgba=[1,0.5,0.5,1])
framenp.append(arrownp)
return task.again
def updateshow(rbd, rbdnp, framenp, task):
for frame in framenp:
frame.detachNode()
dtime = 0.002
angularmomentum = doEulerPhysics(rbd, dtime)
model.setMat(pg.cvtMat4(rbd.rotmat))
# writer.writerow([str(rbd.anglew[0]), str(rbd.anglew[1]), str(rbd.anglew[2])])
writer.writerow([
str(angularmomentum[0]),
str(angularmomentum[1]),
str(angularmomentum[2])
])
arrownp = pg.plotArrow(base.render,
epos=rbd.anglew * 500,
thickness=15)
framenp.append(arrownp)
if task.time > 10.0:
writer.close()
return task.again
pointsnormals.append(eigvec)
return pointsnormals
if __name__=='__main__':
import os
import pandaplotutils.pandactrl as pandactrl
import pandaplotutils.pandageom as pg
import CADTemp
base = pandactrl.World(camp=[0,500,3000], lookatp=[0,0,0])
this_dir, this_filename = os.path.split(__file__)
objpath = os.path.join(this_dir, "models", "calibtable.stl")
cadtemp = CADTemp.CADTemp(ompath=objpath, numpointsoververts=200)
objnp = pg.packpandanp(cadtemp.objtrimesh.vertices,
cadtemp.objtrimesh.face_normals,
cadtemp.objtrimesh.faces,
name='')
temppnts = cadtemp.pcdtemp
normals = estimatenormals(temppnts)
temppntsnp = pg.genPntsnp(temppnts)
temppntsnp.reparentTo(base.render)
for i, normal in enumerate(normals):
pg.plotArrow(base.render, spos = temppnts[i], epos = normal+temppnts[i], thickness = 5, length = 100)
base.run()
def segShow2(self,
base,
togglesamples=False,
togglenormals=False,
togglesamples_ref=False,
togglenormals_ref=False,
togglesamples_refcls=False,
togglenormals_refcls=False,
specificface=True):
"""
:param base:
:param togglesamples:
:param togglenormals:
:param togglesamples_ref: toggles the sampled points that fulfills the dist requirements
:param togglenormals_ref:
:return:
"""
nfacets = self.facets.shape[0]
facetcolorarray = self.facetcolorarray
rgbapnts0 = [1, 1, 1, 1]
rgbapnts1 = [0, 0, 1, 1]
rgbapnts2 = [1, 0, 0, 1]
# offsetf = facet
plotoffsetf = .0
faceplotted = False
# plot the segments
for i, facet in enumerate(self.facets):
if not specificface:
geom = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetf * i * self.facetnormals[i],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[facet],
self.objtrimesh.faces[facet])
node = GeomNode('piece')
node.addGeom(geom)
star = NodePath('piece')
star.attachNewNode(node)
star.setColor(Vec4(.77, .17, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
# sampledpnts = samples[sample_idxes[i]]
# for apnt in sampledpnts:
# pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba)
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(
star,
pos=apnt + plotoffsetf * i * self.facetnormals[i],
radius=2.8,
rgba=rgbapnts0)
if togglenormals:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotArrow(
star,
spos=apnt + plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls[i][j],
rgba=rgbapnts0,
length=10)
if togglesamples_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotSphere(
star,
pos=apnt + plotoffsetf * i * self.facetnormals[i],
radius=2.9,
rgba=rgbapnts1)
if togglenormals_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotArrow(
star,
spos=apnt + plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls_ref[i][j],
rgba=rgbapnts1,
length=10)
if togglesamples_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotSphere(
star,
pos=apnt + plotoffsetf * i * self.facetnormals[i],
radius=3,
rgba=rgbapnts2)
if togglenormals_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotArrow(
star,
spos=apnt + plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls_refcls[i][j],
rgba=rgbapnts2,
length=10)
if specificface:
plotoffsetf = .3
if faceplotted:
continue
else:
if len(self.objsamplepnts[i]) > 85:
faceplotted = True
geom = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetf * i * self.facetnormals[i],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[facet],
self.objtrimesh.faces[facet])
node = GeomNode('piece')
node.addGeom(geom)
star = NodePath('piece')
star.attachNewNode(node)
star.setColor(
Vec4(facetcolorarray[i][0], facetcolorarray[i][1],
facetcolorarray[i][2], 1))
star.setColor(Vec4(.7, .3, .3, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
# sampledpnts = samples[sample_idxes[i]]
# for apnt in sampledpnts:
# pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba)
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(
star,
pos=apnt +
plotoffsetf * i * self.facetnormals[i],
radius=2.8,
rgba=rgbapnts0)
if togglenormals:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotArrow(
star,
spos=apnt +
plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls[i][j],
rgba=rgbapnts0,
length=10)
if togglesamples_ref:
for j, apnt in enumerate(
self.objsamplepnts_ref[i]):
pandageom.plotSphere(
star,
pos=apnt +
plotoffsetf * i * self.facetnormals[i],
radius=2.9,
rgba=rgbapnts1)
if togglenormals_ref:
for j, apnt in enumerate(
self.objsamplepnts_ref[i]):
pandageom.plotArrow(
star,
spos=apnt +
plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls_ref[i][j],
rgba=rgbapnts1,
length=10)
if togglesamples_refcls:
for j, apnt in enumerate(
self.objsamplepnts_refcls[i]):
pandageom.plotSphere(
star,
pos=apnt +
plotoffsetf * i * self.facetnormals[i],
radius=3,
rgba=rgbapnts2)
if togglenormals_refcls:
for j, apnt in enumerate(
self.objsamplepnts_refcls[i]):
pandageom.plotArrow(
star,
spos=apnt +
plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls_refcls[i][j],
rgba=rgbapnts2,
length=10)
def updateshow(ur5u, ur5dualrobot, armid, robotnp, arrownp, counter,
fvalhandinit, tcpinit, tpose, task):
for robot in robotnp:
robot.detachNode()
for arrow in arrownp:
arrow.detachNode()
ur5dualmnp = u5p.genUr5dualmnp(ur5dualrobot, handpkg)
ur5dualmnp.reparentTo(base.render)
robotnp.append(ur5dualmnp)
force = ur5u.getforcevec(armid=armid)
basemat4 = Mat4.rotateMat(-145, Vec3(1, 0, 0))
tcpmat4 = ur5dualrobot.gettcpframe(armid)
fvecx = basemat4.getRow3(0) * force[0]
fvecy = basemat4.getRow3(1) * force[1]
fvecz = basemat4.getRow3(2) * force[2]
fvecxhand = fvecx.project(tcpmat4.getRow3(0)) + fvecy.project(
tcpmat4.getRow3(0)) + fvecz.project(tcpmat4.getRow3(0))
fvecyhand = fvecx.project(tcpmat4.getRow3(1)) + fvecy.project(
tcpmat4.getRow3(1)) + fvecz.project(tcpmat4.getRow3(1))
fveczhand = fvecx.project(tcpmat4.getRow3(2)) + fvecy.project(
tcpmat4.getRow3(2)) + fvecz.project(tcpmat4.getRow3(2))
fvalxhand = fvecxhand.length()
fvalyhand = fvecyhand.length()
fvalzhand = fveczhand.length()
fvecxworld = fvecx.project(Vec3(1, 0, 0)) + fvecy.project(Vec3(
1, 0, 0)) + fvecz.project(Vec3(1, 0, 0))
fvecyworld = fvecx.project(Vec3(0, 1, 0)) + fvecy.project(Vec3(
0, 1, 0)) + fvecz.project(Vec3(0, 1, 0))
fveczworld = fvecx.project(Vec3(0, 0, 1)) + fvecy.project(Vec3(
0, 0, 1)) + fvecz.project(Vec3(0, 0, 1))
fvalxworld = fvecxworld[0]
fvalyworld = fvecyworld[1]
fvalzworld = fveczworld[2]
if counter[0] == 0:
fvalhandinit[0] = fvalxhand
fvalhandinit[1] = fvalyhand
fvalhandinit[2] = fvalzhand
tcpinit[0], tcpinit[1] = ur5dualrobot.gettcp(armid)
counter[0] += 1
task.again
if fvalxhand > fvalhandinit[0] + 20:
deltax = impx.control(fvalxworld)
arrownp.append(
pg.plotArrow(base.render,
tcpmat4.getRow3(3),
tcpmat4.getRow3(3) + fvecxhand * 50,
thickness=10,
rgba=[.7, .2, .2, 1]))
else:
deltax = impx.control(0)
if fvalyhand > fvalhandinit[1] + 20:
deltay = impy.control(fvalyworld)
arrownp.append(
pg.plotArrow(base.render,
tcpmat4.getRow3(3),
tcpmat4.getRow3(3) + fvecyhand * 50,
thickness=10,
rgba=[.2, .7, .2, 1]))
else:
deltay = impy.control(0)
if fvalzhand > fvalhandinit[2] + 20:
deltaz = impz.control(fvalzworld)
arrownp.append(
pg.plotArrow(base.render,
tcpmat4.getRow3(3),
tcpmat4.getRow3(3) + fveczhand * 50,
thickness=10,
rgba=[.2, .2, .7, 1]))
else:
deltaz = impz.control(0)
print tpose[0]
print deltax, deltay, deltaz
tcpgoalpos = tcpinit[0] + np.array([deltax, deltay, deltaz])
tcpgoalrot = tcpinit[1]
armjntsgoal = u5dik.numik(ur5dualrobot, tcpgoalpos, tcpgoalrot, armid)
if armjntsgoal is not None:
ur5dualrobot.movearmfk(armjntsgoal, armid)
# ur5u.movejntssgl(armjntsgoal.tolist(), armid)
return task.again
this_dir, this_filename = os.path.split(__file__)
objpath = os.path.join(this_dir, "models", "ttube.stl")
ppfpath = os.path.join(this_dir, "models", "ttubegmd.pickle")
# ppfmatch = PPFMatch(objpath, ppfpath = None, bsave = True)
ppfmatch = PPFMatch(objpath, ppfpath=ppfpath)
perceivedpnts, perceivednormals = ppfmatch.perceivePoints(1)
# plotperceived pnts
pntsnp = pg.genPntsnp(perceivedpnts)
pntsnp.reparentTo(base.render)
for i, normal in enumerate(perceivednormals):
pg.plotArrow(base.render,
spos=perceivedpnts[i],
epos=normal + perceivedpnts[i],
thickness=.2,
length=10)
rotmat4list, silist, milist = ppfmatch.match(perceivedpnts,
perceivednormals,
ddist=5.0,
dangle=30.0)
try:
for i in range(len(rotmat4list)):
# for i in range(1):
rotmat4 = rotmat4list[i]
si = silist[i]
mi = milist[i]
#object
colors = []
color = [
np.random.rand(),
np.random.rand(),
np.random.rand(),
np.random.rand()
]
for pnt in objectpnts:
colors.append(color)
pntsnp = pg.genPntsnp(objectpnts, colors=colors)
pntsnp.reparentTo(base.render)
for i, normal in enumerate(normals):
pg.plotArrow(base.render,
spos=objectpnts[i],
epos=normal + objectpnts[i],
thickness=.2,
length=10)
# import robotsim.nextage.nxt as nxt
# import robotsim.nextage.nxtplot as nxtplot
# import pandaplotutils.pandageom as pg
# from manipulation.grip.robotiq85 import rtq85nm
# handpkg = rtq85nm
# nxtrobot = nxt.NxtRobot()
# nxtmnp = nxtplot.genmnp(nxtrobot, handpkg)
# nxtmnp.reparentTo(base.render)
# pg.plotAxisSelf(base.render)
# objarray = caliber.getAllObjectPcd()
# for objectpnts in objarray:
def removeFgrpccShow(self, base):
"""
Fgrpcc means finger pre collision detection
This one is specially written for demonstration
:return:
author: weiwei
date: 20161201, osaka
"""
# 6 is used because I am supposing 4+2 where 4 is the default
# margin of bullet in panda3d. (NOTE: This is a guess)
plotoffsetfp = 6
# np0 = base.render.find("**/pair0")
# if np0:
# np0.removeNode()
# np1 = base.render.find("**/pair1")
# if np1:
# np1.removeNode()
#
# np0collection = base.render.findAllMatches("**/rtq85fgrpcc0")
# for np0 in np0collection:
# np0.removeNode()
# np1collection = base.render.findAllMatches("**/rtq85fgrpcc1")
# for np1 in np1collection:
# np1.removeNode()
#
# npscollection = base.render.findAllMatches("**/sphere")
# for nps in npscollection:
# nps.removeNode()
npbrchild = base.render.find("**/tempplot")
if npbrchild:
npbrchild.removeNode()
# for fast delete
brchild = NodePath('tempplot')
brchild.reparentTo(base.render)
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom(self.objtrimesh.vertices+
np.tile(plotoffsetfp*self.facetnormals[facetidx0],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[self.facets[facetidx0]],
self.objtrimesh.faces[self.facets[facetidx0]])
geomfacet1 = pandageom.packpandageom(self.objtrimesh.vertices+
np.tile(plotoffsetfp*self.facetnormals[facetidx1],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[self.facets[facetidx1]],
self.objtrimesh.faces[self.facets[facetidx1]])
# show the facetpair
node0 = GeomNode('pair0')
node0.addGeom(geomfacet0)
star0 = NodePath('pair0')
star0.attachNewNode(node0)
facetcolorarray = self.facetcolorarray
star0.setColor(Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star0.setTwoSided(True)
star0.reparentTo(brchild)
node1 = GeomNode('pair1')
node1.addGeom(geomfacet1)
star1 = NodePath('pair1')
star1.attachNewNode(node1)
star1.setColor(Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]))
star1.setTwoSided(True)
star1.reparentTo(brchild)
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
# the following two choices decide the way to detect contacts
cctnormal00 = np.array(self.gripcontactpairnormals[self.counter][j][0])
cctnormal01 = -np.array(self.gripcontactpairnormals[self.counter][j][1])
cctnormal0raw = (cctnormal00 + cctnormal01)
cctnormal0 = (cctnormal0raw/np.linalg.norm(cctnormal0raw)).tolist()
# the following two choices decide the way to detect contacts
cctnormal10 = -cctnormal00
cctnormal11 = -cctnormal01
cctnormal1raw = (cctnormal10 + cctnormal11)
cctnormal1 = (cctnormal1raw/np.linalg.norm(cctnormal1raw)).tolist()
handfgrpcc0 = NodePath("handfgrpcc0")
self.handfgrpcc_uninstanced.instanceTo(rtq85pcc0)
handfgrpcc0.setPos(cctpnt0[0], cctpnt0[1], cctpnt0[2])
handfgrpcc0.lookAt(cctpnt0[0] + cctnormal0[0], cctpnt0[1] + cctnormal0[1], cctpnt0[2] + cctnormal0[2])
handfgrpcc1 = NodePath("handfgrpcc1")
self.handfgrpcc_uninstanced.instanceTo(rtq85pcc1)
handfgrpcc1.setPos(cctpnt1[0], cctpnt1[1], cctpnt1[2])
handfgrpcc1.lookAt(cctpnt1[0] + cctnormal1[0], cctpnt1[1] + cctnormal1[1], cctpnt1[2] + cctnormal1[2])
handfgrpcc = NodePath("handfgrpcc")
handfgrpcc0.reparentTo(handfgrpcc)
handfgrpcc1.reparentTo(handfgrpcc)
# prepare the model for collision detection
facetmeshbullnode = cd.genCollisionMeshMultiNp(handfgrpcc, brchild)
result = self.bulletworld.contactTest(facetmeshbullnode)
for contact in result.getContacts():
cp = contact.getManifoldPoint()
pandageom.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1))
pandageom.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1))
if result.getNumContacts():
handfgrpcc0.setColor(1, 0, 0, .3)
handfgrpcc1.setColor(1, 0, 0, .3)
else:
handfgrpcc0.setColor(1, 1, 1, .3)
handfgrpcc1.setColor(1, 1, 1, .3)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(brchild)
handfgrpcc1.reparentTo(brchild)
pandageom.plotArrow(star0, spos=cctpnt0,
epos=cctpnt0 + plotoffsetfp*self.facetnormals[facetidx0] + cctnormal0,
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]], length=10)
pandageom.plotArrow(star1, spos=cctpnt1,
epos=cctpnt1 + plotoffsetfp*self.facetnormals[facetidx1] + cctnormal1,
rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]], length=10)
def segShow2(self, base, togglesamples=False, togglenormals=False,
togglesamples_ref=False, togglenormals_ref=False,
togglesamples_refcls=False, togglenormals_refcls=False, specificface = True):
"""
:param base:
:param togglesamples:
:param togglenormals:
:param togglesamples_ref: toggles the sampled points that fulfills the dist requirements
:param togglenormals_ref:
:return:
"""
nfacets = self.facets.shape[0]
facetcolorarray = self.facetcolorarray
rgbapnts0 = [1, 1, 1, 1]
rgbapnts1 = [0.2, 0.7, 1, 1]
rgbapnts2 = [1, 0.7, 0.2, 1]
# offsetf = facet
plotoffsetf = .0
faceplotted = False
# plot the segments
for i, facet in enumerate(self.facets):
if not specificface:
geom = pandageom.packpandageom(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[facet], self.objtrimesh.faces[facet])
node = GeomNode('piece')
node.addGeom(geom)
star = NodePath('piece')
star.attachNewNode(node)
star.setColor(Vec4(.77, .17, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
# sampledpnts = samples[sample_idxes[i]]
# for apnt in sampledpnts:
# pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba)
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.8, rgba=rgbapnts0)
if togglenormals:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls[i][j],
rgba=rgbapnts0, length=10)
if togglesamples_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.9, rgba=rgbapnts1)
if togglenormals_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_ref[i][j],
rgba=rgbapnts1, length=10)
if togglesamples_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts2)
if togglenormals_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_refcls[i][j],
rgba=rgbapnts2, length=10)
if specificface:
plotoffsetf = .1
if faceplotted:
continue
else:
if len(self.objsamplepnts[i])>25:
faceplotted = True
geom = pandageom.packpandageom(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[facet], self.objtrimesh.faces[facet])
node = GeomNode('piece')
node.addGeom(geom)
star = NodePath('piece')
star.attachNewNode(node)
star.setColor(Vec4(facetcolorarray[i][0], facetcolorarray[i][1], facetcolorarray[i][2], 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
# sampledpnts = samples[sample_idxes[i]]
# for apnt in sampledpnts:
# pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba)
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.8, rgba=rgbapnts0)
if togglenormals:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls[i][j],
rgba=rgbapnts0, length=10)
if togglesamples_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts1)
if togglenormals_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_ref[i][j],
rgba=rgbapnts1, length=10)
if togglesamples_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3.5, rgba=rgbapnts2)
if togglenormals_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_refcls[i][j],
rgba=rgbapnts2, length=10)
def removebadfacetsshow(self, base, doverh=.1):
"""
remove the facets that cannot support stable placements
:param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
when f>0.1mg, the object is judged to be unstable
:return:
author: weiwei
date: 20161213
"""
plotoffsetfp = 10
# print self.counter
if self.counter < len(self.ocfacets):
i = self.counter
# for i in range(len(self.ocfacets)):
geom = pg.packpandageom(self.objtrimeshconv.vertices,
self.objtrimeshconv.face_normals[self.ocfacets[i]],
self.objtrimeshconv.faces[self.ocfacets[i]])
geombullnode = cd.genCollisionMeshGeom(geom)
self.bulletworldray.attachRigidBody(geombullnode)
pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
pTo = self.objcom+self.ocfacetnormals[i]*99999
pTo = Point3(pTo[0], pTo[1], pTo[2])
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
self.bulletworldray.removeRigidBody(geombullnode)
if result.hasHit():
hitpos = result.getHitPos()
pg.plotArrow(base.render, spos=self.objcom,
epos = self.objcom+self.ocfacetnormals[i], length=100)
facetinterpnt = np.array([hitpos[0],hitpos[1],hitpos[2]])
facetnormal = np.array(self.ocfacetnormals[i])
bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(self.objtrimeshconv, self.ocfacets[i],
facetinterpnt, facetnormal)
for j in range(len(bdverts3d)-1):
spos = bdverts3d[j]
epos = bdverts3d[j+1]
pg.plotStick(base.render, spos, epos, thickness = 1, rgba=[.5,.5,.5,1])
facetp = Polygon(bdverts2d)
facetinterpnt2d = rm.transformmat4(facetmat4, facetinterpnt)[:2]
apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
dist2p = apntpnt.distance(facetp.exterior)
dist2c = np.linalg.norm(np.array([hitpos[0],hitpos[1],hitpos[2]])-np.array([pFrom[0],pFrom[1],pFrom[2]]))
if dist2p/dist2c < doverh:
print "not stable"
# return
else:
print dist2p/dist2c
pol_ext = LinearRing(bdverts2d)
d = pol_ext.project(apntpnt)
p = pol_ext.interpolate(d)
closest_point_coords = list(p.coords)[0]
closep = np.array([closest_point_coords[0], closest_point_coords[1], 0])
closep3d = rm.transformmat4(rm.homoinverse(facetmat4), closep)[:3]
pg.plotDumbbell(base.render, spos=facetinterpnt, epos=closep3d, thickness=1.5, rgba=[0,0,1,1])
for j in range(len(bdverts3d)-1):
spos = bdverts3d[j]
epos = bdverts3d[j+1]
pg.plotStick(base.render, spos, epos, thickness = 1.5, rgba=[0,1,0,1])
# geomoff = pg.packpandageom(self.objtrimeshconv.vertices +
# np.tile(plotoffsetfp * self.ocfacetnormals[i],
# [self.objtrimeshconv.vertices.shape[0], 1]),
# self.objtrimeshconv.face_normals[self.ocfacets[i]],
# self.objtrimeshconv.faces[self.ocfacets[i]])
#
# nodeoff = GeomNode('supportfacet')
# nodeoff.addGeom(geomoff)
# staroff = NodePath('supportfacet')
# staroff.attachNewNode(nodeoff)
# staroff.setColor(Vec4(1,0,1,1))
# staroff.setTransparency(TransparencyAttrib.MAlpha)
# staroff.setTwoSided(True)
# staroff.reparentTo(base.render)
self.counter+=1
else:
self.counter=0
def pairShow(self, base, togglecontacts = False, togglecontactnormals = False):
# the following sentence requires segshow to be executed first
facetcolorarray = self.facetcolorarray
# offsetfp = facetpair
plotoffsetfp = np.random.random()*50
# plot the pairs and their contacts
# for i in range(self.counter+1, len(self.facetpairs)):
# if self.gripcontactpairs[i]:
# self.counter = i
# break
# if i is len(self.facetpairs):
# return
# delete the facetpair after show
# np0 = base.render.find("**/pair0")
# if np0:
# np0.removeNode()
# np1 = base.render.find("**/pair1")
# if np1:
# np1.removeNode()
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
# self.counter = 0
return
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom(self.objtrimesh.vertices+
np.tile(plotoffsetfp*self.facetnormals[facetidx0],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[self.facets[facetidx0]],
self.objtrimesh.faces[self.facets[facetidx0]])
geomfacet1 = pandageom.packpandageom(self.objtrimesh.vertices+
np.tile(plotoffsetfp*self.facetnormals[facetidx1],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[self.facets[facetidx1]],
self.objtrimesh.faces[self.facets[facetidx1]])
# show the facetpair
node0 = GeomNode('pair0')
node0.addGeom(geomfacet0)
star0 = NodePath('pair0')
star0.attachNewNode(node0)
star0.setColor(Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star0.setTwoSided(True)
star0.reparentTo(base.render)
node1 = GeomNode('pair1')
node1.addGeom(geomfacet1)
star1 = NodePath('pair1')
star1.attachNewNode(node1)
# star1.setColor(Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]))
# set to the same color
star1.setColor(Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star1.setTwoSided(True)
star1.reparentTo(base.render)
if togglecontacts:
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cttpnt0 = contactpair[0]
cttpnt1 = contactpair[1]
pandageom.plotSphere(star0, pos=cttpnt0+plotoffsetfp*self.facetnormals[facetidx0], radius=4,
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]])
# pandageom.plotSphere(star1, pos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], radius=4,
# rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]])
# use the same color
pandageom.plotSphere(star1, pos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], radius=4,
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]])
if togglecontactnormals:
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cttpnt0 = contactpair[0]
cttpnt1 = contactpair[1]
pandageom.plotArrow(star0, spos=cttpnt0+plotoffsetfp*self.facetnormals[facetidx0],
epos=cttpnt0 + plotoffsetfp*self.facetnormals[facetidx0] +
self.gripcontactpairnormals[self.counter][j][0],
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]], length=10)
# pandageom.plotArrow(star1, spos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1],
# epos=cttpnt1 + plotoffsetfp*self.facetnormals[facetidx1] +
# self.gripcontactpairnormals[self.counter][j][1],
# rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]], length=10)
# use the same color
pandageom.plotArrow(star1, spos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1],
epos=cttpnt1 + plotoffsetfp*self.facetnormals[facetidx1] +
self.gripcontactpairnormals[self.counter][j][1],
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]], length=10)
self.__newsamplesnoverts.append(sample)
self.__newsamplesnovertsnormals.append(normal)
else:
continue
if __name__=='__main__':
base = pandactrl.World(camp=[0,0,300], lookatp=[0,0,0])
this_dir, this_filename = os.path.split(__file__)
objpath = os.path.join(this_dir, "models", "ttube.stl")
cadtemp = CADTemp(ompath = objpath)
objnp = pg.packpandanp(cadtemp.objtrimesh.vertices,
cadtemp.objtrimesh.face_normals,
cadtemp.objtrimesh.faces,
name='')
# objnp.reparentTo(base.render)
colors = []
color = [1,0,0,1]
for pnt in cadtemp.pcdtempnovertsinner:
colors.append(color)
pntsnp = pg.genPntsnp(cadtemp.pcdtempnovertsinner, colors=colors)
pntsnp.reparentTo(base.render)
for i, normal in enumerate(cadtemp.pcdtempnovertsnormalsinner):
pg.plotArrow(base.render, spos = cadtemp.pcdtempnovertsinner[i],
epos = normal+cadtemp.pcdtempnovertsinner[i], thickness = .2, length =10)
base.run()
if __name__ == '__main__':
import os
import pandaplotutils.pandactrl as pandactrl
import pandaplotutils.pandageom as pg
import CADTemp
base = pandactrl.World(camp=[0, 500, 3000], lookatp=[0, 0, 0])
this_dir, this_filename = os.path.split(__file__)
objpath = os.path.join(this_dir, "models", "calibtable.stl")
cadtemp = CADTemp.CADTemp(ompath=objpath, numpointsoververts=200)
objnp = pg.packpandanp(cadtemp.objtrimesh.vertices,
cadtemp.objtrimesh.face_normals,
cadtemp.objtrimesh.faces,
name='')
temppnts = cadtemp.pcdtemp
normals = estimatenormals(temppnts)
temppntsnp = pg.genPntsnp(temppnts)
temppntsnp.reparentTo(base.render)
for i, normal in enumerate(normals):
pg.plotArrow(base.render,
spos=temppnts[i],
epos=normal + temppnts[i],
thickness=5,
length=100)
base.run()
base = pandactrl.World(camp=[0,0,700], lookatp=[0,0,0])
this_dir, this_filename = os.path.split(__file__)
objpath = os.path.join(this_dir, "models", "ttube.stl")
ppfpath = os.path.join(this_dir, "models", "ttubegmd.pickle")
# ppfmatch = PPFMatch(objpath, ppfpath = None, bsave = True)
ppfmatch = PPFMatch(objpath, ppfpath = ppfpath)
perceivedpnts, perceivednormals = ppfmatch.perceivePoints(1)
# plotperceived pnts
pntsnp = pg.genPntsnp(perceivedpnts)
pntsnp.reparentTo(base.render)
for i, normal in enumerate(perceivednormals):
pg.plotArrow(base.render, spos = perceivedpnts[i], epos = normal+perceivedpnts[i], thickness = .2, length = 10)
rotmat4list, silist, milist = ppfmatch.match(perceivedpnts, perceivednormals, ddist = 5.0, dangle = 30.0)
try:
for i in range(len(rotmat4list)):
# for i in range(1):
rotmat4 = rotmat4list[i]
si = silist[i]
mi = milist[i]
#object
# ppfmatch.objnp.setMat(rotmat4)
# ppfmatch.objnp.reparentTo(base.render)
# point arrow
pg.plotArrow(base.render, spos = perceivedpnts[si], epos = perceivednormals[si]+perceivedpnts[si], thickness = 4, length = 50)
# temp arrow
def updateshow(ur5u, ur5dualrobot, armid, robotnp, arrownp, counter, fvalhandinit, tcpinit, tpose, task):
for robot in robotnp:
robot.detachNode()
for arrow in arrownp:
arrow.detachNode()
ur5dualmnp = u5p.genmnp(ur5dualrobot, handpkg)
ur5dualmnp.reparentTo(base.render)
robotnp.append(ur5dualmnp)
force = ur5u.getforcevec(armid = armid)
basemat4 = Mat4.rotateMat(-145, Vec3(1,0,0))
tcpmat4 = ur5dualrobot.gettcpframe(armid)
fvecx = basemat4.getRow3(0)*force[0]
fvecy = basemat4.getRow3(1)*force[1]
fvecz = basemat4.getRow3(2)*force[2]
fvecxhand = fvecx.project(tcpmat4.getRow3(0)) + fvecy.project(tcpmat4.getRow3(0)) + fvecz.project(tcpmat4.getRow3(0))
fvecyhand = fvecx.project(tcpmat4.getRow3(1)) + fvecy.project(tcpmat4.getRow3(1)) + fvecz.project(tcpmat4.getRow3(1))
fveczhand = fvecx.project(tcpmat4.getRow3(2)) + fvecy.project(tcpmat4.getRow3(2)) + fvecz.project(tcpmat4.getRow3(2))
fvalxhand = fvecxhand.length()
fvalyhand = fvecyhand.length()
fvalzhand = fveczhand.length()
fvecxworld = fvecx.project(Vec3(1,0,0)) + fvecy.project(Vec3(1,0,0)) + fvecz.project(Vec3(1,0,0))
fvecyworld = fvecx.project(Vec3(0,1,0)) + fvecy.project(Vec3(0,1,0)) + fvecz.project(Vec3(0,1,0))
fveczworld = fvecx.project(Vec3(0,0,1)) + fvecy.project(Vec3(0,0,1)) + fvecz.project(Vec3(0,0,1))
fvalxworld = fvecxworld[0]
fvalyworld = fvecyworld[1]
fvalzworld = fveczworld[2]
if counter[0] == 0:
fvalhandinit[0] = fvalxhand
fvalhandinit[1] = fvalyhand
fvalhandinit[2] = fvalzhand
tcpinit[0], tcpinit[1] = ur5dualrobot.gettcp(armid)
counter[0] += 1
task.again
if fvalxhand > fvalhandinit[0]+20:
deltax = impx.control(fvalxworld)
arrownp.append(pg.plotArrow(base.render, tcpmat4.getRow3(3), tcpmat4.getRow3(3)+fvecxhand*50, thickness = 10, rgba = [.7,.2,.2,1]))
else:
deltax = impx.control(0)
if fvalyhand > fvalhandinit[1]+20:
deltay = impy.control(fvalyworld)
arrownp.append(pg.plotArrow(base.render, tcpmat4.getRow3(3), tcpmat4.getRow3(3)+fvecyhand*50, thickness = 10, rgba = [.2,.7,.2,1]))
else:
deltay = impy.control(0)
if fvalzhand > fvalhandinit[2]+20:
deltaz = impz.control(fvalzworld)
arrownp.append(pg.plotArrow(base.render, tcpmat4.getRow3(3), tcpmat4.getRow3(3)+fveczhand*50, thickness = 10, rgba = [.2,.2,.7,1]))
else:
deltaz = impz.control(0)
print tpose[0]
print deltax, deltay, deltaz
tcpgoalpos = tcpinit[0] + np.array([deltax, deltay, deltaz])
tcpgoalrot = tcpinit[1]
armjntsgoal = u5dik.numik(ur5dualrobot, tcpgoalpos, tcpgoalrot, armid)
if armjntsgoal is not None:
ur5dualrobot.movearmfk(armjntsgoal, armid)
# ur5u.movejntssgl(armjntsgoal.tolist(), armid)
return task.again
def removebadfacetsshow(self, base, doverh=.1):
"""
remove the facets that cannot support stable placements
:param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
when f>0.1mg, the object is judged to be unstable
:return:
author: weiwei
date: 20161213
"""
plotoffsetfp = 10
# print self.counter
if self.counter < len(self.ocfacets):
i = self.counter
# for i in range(len(self.ocfacets)):
geom = pg.packpandageom(
self.objtrimeshconv.vertices,
self.objtrimeshconv.face_normals[self.ocfacets[i]],
self.objtrimeshconv.faces[self.ocfacets[i]])
geombullnode = cd.genCollisionMeshGeom(geom)
self.bulletworldray.attachRigidBody(geombullnode)
pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
pTo = self.objcom + self.ocfacetnormals[i] * 99999
pTo = Point3(pTo[0], pTo[1], pTo[2])
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
self.bulletworldray.removeRigidBody(geombullnode)
if result.hasHit():
hitpos = result.getHitPos()
pg.plotArrow(base.render,
spos=self.objcom,
epos=self.objcom + self.ocfacetnormals[i],
length=100)
facetinterpnt = np.array([hitpos[0], hitpos[1], hitpos[2]])
facetnormal = np.array(self.ocfacetnormals[i])
bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(
self.objtrimeshconv, self.ocfacets[i], facetinterpnt,
facetnormal)
for j in range(len(bdverts3d) - 1):
spos = bdverts3d[j]
epos = bdverts3d[j + 1]
pg.plotStick(base.render,
spos,
epos,
thickness=1,
rgba=[.5, .5, .5, 1])
facetp = Polygon(bdverts2d)
facetinterpnt2d = rm.transformmat4(facetmat4,
facetinterpnt)[:2]
apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
dist2p = apntpnt.distance(facetp.exterior)
dist2c = np.linalg.norm(
np.array([hitpos[0], hitpos[1], hitpos[2]]) -
np.array([pFrom[0], pFrom[1], pFrom[2]]))
if dist2p / dist2c < doverh:
print "not stable"
# return
else:
pol_ext = LinearRing(bdverts2d)
d = pol_ext.project(apntpnt)
p = pol_ext.interpolate(d)
closest_point_coords = list(p.coords)[0]
closep = np.array(
[closest_point_coords[0], closest_point_coords[1], 0])
closep3d = rm.transformmat4(rm.homoinverse(facetmat4),
closep)[:3]
pg.plotDumbbell(base.render,
spos=facetinterpnt,
epos=closep3d,
thickness=1.5,
rgba=[0, 0, 1, 1])
for j in range(len(bdverts3d) - 1):
spos = bdverts3d[j]
epos = bdverts3d[j + 1]
pg.plotStick(base.render,
spos,
epos,
thickness=1.5,
rgba=[0, 1, 0, 1])
# geomoff = pg.packpandageom(self.objtrimeshconv.vertices +
# np.tile(plotoffsetfp * self.ocfacetnormals[i],
# [self.objtrimeshconv.vertices.shape[0], 1]),
# self.objtrimeshconv.face_normals[self.ocfacets[i]],
# self.objtrimeshconv.faces[self.ocfacets[i]])
#
# nodeoff = GeomNode('supportfacet')
# nodeoff.addGeom(geomoff)
# staroff = NodePath('supportfacet')
# staroff.attachNewNode(nodeoff)
# staroff.setColor(Vec4(1,0,1,1))
# staroff.setTransparency(TransparencyAttrib.MAlpha)
# staroff.setTwoSided(True)
# staroff.reparentTo(base.render)
self.counter += 1
else:
self.counter = 0
def computePPFwithAlpha(self, ddist=5.0, dangle=2 * math.pi / 12.0):
"""
compute the point pair feature f1, f2, f3, f4,
and the alpha_m
:return: a dictionary
author: weiwei
date: 20170714
"""
# global model descriptor, gmd
gmd = {}
ntemppoint = self.temppnts.shape[0]
for i in range(ntemppoint):
print i, ntemppoint
for j in range(ntemppoint):
# for i in range(0,1):
# for j in range(3,4):
m_0 = np.asarray(self.temppnts[i])
m_1 = np.asarray(self.temppnts[j])
v_m0m1 = m_0 - m_1
v_m1m0 = m_1 - m_0
n_m0 = self.tempnormals[i]
n_m1 = self.tempnormals[j]
# f1, namely ||d||2
f1 = np.linalg.norm(m_0 - m_1)
# f2, namely angle between n_m0 and v_m1m0
f2 = rm.radian_between(n_m0, v_m1m0)
# f3, namely angle between n_m1 and v_m0m1
f3 = rm.radian_between(n_m1, v_m0m1)
# f4, namely angle between n_m0 and n_m1
f4 = rm.radian_between(n_m0, n_m1)
# discretize the values
f1d = math.floor(f1 / ddist) * ddist + ddist
f2d = math.floor(f2 / dangle) * dangle + dangle
f3d = math.floor(f3 / dangle) * dangle + dangle
f4d = math.floor(f4 / dangle) * dangle + dangle
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
# debug
# before transform
pg.plotArrow(base.render,
spos=m_0,
epos=m_1,
rgba=Vec4(0, 1, 0, 1))
pg.plotArrow(base.render,
spos=m_0,
epos=m_0 + n_m0,
rgba=Vec4(1, 0, 0, 1))
# after transform
# print v_m1m0onxplus
# print v_m1m0onxplusyzproj
pg.plotArrow(base.render,
spos=m_0,
epos=v_m1m0onxplus + m_0,
rgba=Vec4(0, .7, .7, 1))
pg.plotArrow(base.render,
spos=m_0,
epos=v_m1m0onxplusyzproj + m_0,
rgba=Vec4(.70, .7, .7, 1))
pg.plotArrow(base.render,
spos=m_0,
epos=m_0 + xplus,
rgba=Vec4(.7, 0, .7, 1))
# alpha_m0
# print np.degrees(alpha_m0)
# plot aixs
zplus = np.asarray([0, 0, 1])
pg.plotArrow(base.render,
spos=m_0,
epos=m_0 + xplus * 10,
rgba=Vec4(.3, 0, 0, .3))
pg.plotArrow(base.render,
spos=m_0,
epos=m_0 + yplus * 10,
rgba=Vec4(0, .3, 0, .3))
pg.plotArrow(base.render,
spos=m_0,
epos=m_0 + zplus * 10,
rgba=Vec4(0, 0, .3, .3))
if key in gmd.keys():
gmd[key].append([m_0, m_1, alpha_m0])
else:
gmd[key] = [[m_0, m_1, alpha_m0]]
def removeFgrpccShow(self, base):
"""
Fgrpcc means finger pre collision detection
This one is specially written for demonstration
:return:
author: weiwei
date: 20161201, osaka
"""
# 6 is used because I am supposing 4+2 where 4 is the default
# margin of bullet in panda3d. (NOTE: This is a guess)
plotoffsetfp = 6
# np0 = base.render.find("**/pair0")
# if np0:
# np0.removeNode()
# np1 = base.render.find("**/pair1")
# if np1:
# np1.removeNode()
#
# np0collection = base.render.findAllMatches("**/rtq85fgrpcc0")
# for np0 in np0collection:
# np0.removeNode()
# np1collection = base.render.findAllMatches("**/rtq85fgrpcc1")
# for np1 in np1collection:
# np1.removeNode()
#
# npscollection = base.render.findAllMatches("**/sphere")
# for nps in npscollection:
# nps.removeNode()
npbrchild = base.render.find("**/tempplot")
if npbrchild:
npbrchild.removeNode()
# for fast delete
brchild = NodePath('tempplot')
brchild.reparentTo(base.render)
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetfp * self.facetnormals[facetidx0],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[self.facets[facetidx0]],
self.objtrimesh.faces[self.facets[facetidx0]])
geomfacet1 = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetfp * self.facetnormals[facetidx1],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[self.facets[facetidx1]],
self.objtrimesh.faces[self.facets[facetidx1]])
# show the facetpair
node0 = GeomNode('pair0')
node0.addGeom(geomfacet0)
star0 = NodePath('pair0')
star0.attachNewNode(node0)
facetcolorarray = self.facetcolorarray
star0.setColor(
Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star0.setTwoSided(True)
star0.reparentTo(brchild)
node1 = GeomNode('pair1')
node1.addGeom(geomfacet1)
star1 = NodePath('pair1')
star1.attachNewNode(node1)
star1.setColor(
Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]))
star1.setTwoSided(True)
star1.reparentTo(brchild)
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
# the following two choices decide the way to detect contacts
cctnormal00 = np.array(
self.gripcontactpairnormals[self.counter][j][0])
cctnormal01 = -np.array(
self.gripcontactpairnormals[self.counter][j][1])
cctnormal0raw = (cctnormal00 + cctnormal01)
cctnormal0 = (cctnormal0raw /
np.linalg.norm(cctnormal0raw)).tolist()
# the following two choices decide the way to detect contacts
cctnormal10 = -cctnormal00
cctnormal11 = -cctnormal01
cctnormal1raw = (cctnormal10 + cctnormal11)
cctnormal1 = (cctnormal1raw /
np.linalg.norm(cctnormal1raw)).tolist()
handfgrpcc0 = NodePath("handfgrpcc0")
self.handfgrpcc_uninstanced.instanceTo(rtq85pcc0)
handfgrpcc0.setPos(cctpnt0[0], cctpnt0[1], cctpnt0[2])
handfgrpcc0.lookAt(cctpnt0[0] + cctnormal0[0],
cctpnt0[1] + cctnormal0[1],
cctpnt0[2] + cctnormal0[2])
handfgrpcc1 = NodePath("handfgrpcc1")
self.handfgrpcc_uninstanced.instanceTo(rtq85pcc1)
handfgrpcc1.setPos(cctpnt1[0], cctpnt1[1], cctpnt1[2])
handfgrpcc1.lookAt(cctpnt1[0] + cctnormal1[0],
cctpnt1[1] + cctnormal1[1],
cctpnt1[2] + cctnormal1[2])
handfgrpcc = NodePath("handfgrpcc")
handfgrpcc0.reparentTo(handfgrpcc)
handfgrpcc1.reparentTo(handfgrpcc)
# prepare the model for collision detection
facetmeshbullnode = cd.genCollisionMeshMultiNp(handfgrpcc, brchild)
result = self.bulletworld.contactTest(facetmeshbullnode)
for contact in result.getContacts():
cp = contact.getManifoldPoint()
pandageom.plotSphere(brchild,
pos=cp.getLocalPointA(),
radius=3,
rgba=Vec4(1, 0, 0, 1))
pandageom.plotSphere(brchild,
pos=cp.getLocalPointB(),
radius=3,
rgba=Vec4(0, 0, 1, 1))
if result.getNumContacts():
handfgrpcc0.setColor(1, 0, 0, .3)
handfgrpcc1.setColor(1, 0, 0, .3)
else:
handfgrpcc0.setColor(1, 1, 1, .3)
handfgrpcc1.setColor(1, 1, 1, .3)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(brchild)
handfgrpcc1.reparentTo(brchild)
pandageom.plotArrow(star0,
spos=cctpnt0,
epos=cctpnt0 +
plotoffsetfp * self.facetnormals[facetidx0] +
cctnormal0,
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
],
length=10)
pandageom.plotArrow(star1,
spos=cctpnt1,
epos=cctpnt1 +
plotoffsetfp * self.facetnormals[facetidx1] +
cctnormal1,
rgba=[
facetcolorarray[facetidx1][0],
facetcolorarray[facetidx1][1],
facetcolorarray[facetidx1][2],
facetcolorarray[facetidx1][3]
],
length=10)
# # normals
normals = tools.estimatenormals(objectpnts)
# transform
objectpntscenter = np.mean(objectpnts, axis=0)
for i in range(objectpnts.shape[0]):
objectpnts[i] = objectpnts[i]-objectpntscenter
colors = []
color = [np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand()]
for pnt in objectpnts:
colors.append(color)
pntsnp = pg.genPntsnp(objectpnts, colors=colors)
pntsnp.reparentTo(base.render)
for i, normal in enumerate(normals):
pg.plotArrow(base.render, spos = objectpnts[i], epos = normal+objectpnts[i], thickness = .2, length = 2)
# import robotsim.nextage.nxt as nxt
# import robotsim.nextage.nxtplot as nxtplot
# import pandaplotutils.pandageom as pg
# from manipulation.grip.robotiq85 import rtq85nm
# handpkg = rtq85nm
# nxtrobot = nxt.NxtRobot()
# nxtmnp = nxtplot.genmnp(nxtrobot, handpkg)
# nxtmnp.reparentTo(base.render)
# pg.plotAxisSelf(base.render)
# objarray = kface.getAllObjectPcd()
# for objectpnts in objarray:
# colors = []
# color = [np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand()]
def pairShow(self, base, togglecontacts=False, togglecontactnormals=False):
# the following sentence requires segshow to be executed first
facetcolorarray = self.facetcolorarray
# offsetfp = facetpair
plotoffsetfp = np.random.random() * 50
# plot the pairs and their contacts
# for i in range(self.counter+1, len(self.facetpairs)):
# if self.gripcontactpairs[i]:
# self.counter = i
# break
# if i is len(self.facetpairs):
# return
# delete the facetpair after show
# np0 = base.render.find("**/pair0")
# if np0:
# np0.removeNode()
# np1 = base.render.find("**/pair1")
# if np1:
# np1.removeNode()
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
# self.counter = 0
return
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetfp * self.facetnormals[facetidx0],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[self.facets[facetidx0]],
self.objtrimesh.faces[self.facets[facetidx0]])
geomfacet1 = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetfp * self.facetnormals[facetidx1],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[self.facets[facetidx1]],
self.objtrimesh.faces[self.facets[facetidx1]])
# show the facetpair
node0 = GeomNode('pair0')
node0.addGeom(geomfacet0)
star0 = NodePath('pair0')
star0.attachNewNode(node0)
star0.setColor(
Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star0.setTwoSided(True)
star0.reparentTo(base.render)
node1 = GeomNode('pair1')
node1.addGeom(geomfacet1)
star1 = NodePath('pair1')
star1.attachNewNode(node1)
# star1.setColor(Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]))
# set to the same color
star1.setColor(
Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star1.setTwoSided(True)
star1.reparentTo(base.render)
if togglecontacts:
for j, contactpair in enumerate(
self.gripcontactpairs[self.counter]):
cttpnt0 = contactpair[0]
cttpnt1 = contactpair[1]
pandageom.plotSphere(
star0,
pos=cttpnt0 + plotoffsetfp * self.facetnormals[facetidx0],
radius=4,
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
])
# pandageom.plotSphere(star1, pos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], radius=4,
# rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]])
# use the same color
pandageom.plotSphere(
star1,
pos=cttpnt1 + plotoffsetfp * self.facetnormals[facetidx1],
radius=4,
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
])
if togglecontactnormals:
for j, contactpair in enumerate(
self.gripcontactpairs[self.counter]):
cttpnt0 = contactpair[0]
cttpnt1 = contactpair[1]
pandageom.plotArrow(
star0,
spos=cttpnt0 + plotoffsetfp * self.facetnormals[facetidx0],
epos=cttpnt0 +
plotoffsetfp * self.facetnormals[facetidx0] +
self.gripcontactpairnormals[self.counter][j][0],
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
],
length=10)
# pandageom.plotArrow(star1, spos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1],
# epos=cttpnt1 + plotoffsetfp*self.facetnormals[facetidx1] +
# self.gripcontactpairnormals[self.counter][j][1],
# rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]], length=10)
# use the same color
pandageom.plotArrow(
star1,
spos=cttpnt1 + plotoffsetfp * self.facetnormals[facetidx1],
epos=cttpnt1 +
plotoffsetfp * self.facetnormals[facetidx1] +
self.gripcontactpairnormals[self.counter][j][1],
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
],
length=10)
if __name__ == '__main__':
base = pandactrl.World(camp=[0, 0, 700], lookatp=[0, 0, 0])
this_dir, this_filename = os.path.split(__file__)
objpath = os.path.join(this_dir, "models", "ttube.stl")
ppfmatch = PPFMatch(objpath)
# ppfmatch.computePPFwithAlpha()
colors = []
color = [
np.random.rand(),
np.random.rand(),
np.random.rand(),
np.random.rand()
]
for pnt in ppfmatch.temppnts:
colors.append(color)
pntsnp = pg.genPntsnp(ppfmatch.temppnts, colors=colors)
for i, normal in enumerate(ppfmatch.tempnormals):
pg.plotArrow(base.render,
spos=ppfmatch.temppnts[i],
epos=normal + ppfmatch.temppnts[i],
thickness=.2,
length=2)
# pg.plotAxisSelf(base.render)
base.run()
