def plotstick(pandanp,
hrp5robot,
rgtrbga=np.array([.5, 0, 0, 0]),
lftrgba=np.array([.5, 0, 0, 0])):
"""
plot the stick model of the nextage robot in panda3d
:param pandanp: a panda3d nodepath
:param hrp5robot: the Hrp5Robot object, see Hrp5robot.py
:param rgtrbga: color of right arm
:param lftrgba: color of left arm
:return: null
author: weiwei
date: 20161202
"""
i = 0
while i != -1:
pg.plotDumbbell(pandanp,
spos=hrp5robot.rgtarm[i]['linkpos'],
epos=hrp5robot.rgtarm[i]['linkend'],
thickness=20,
rgba=rgtrbga)
i = hrp5robot.rgtarm[i]['child']
i = 0
while i != -1:
pg.plotDumbbell(pandanp,
spos=hrp5robot.lftarm[i]['linkpos'],
epos=hrp5robot.lftarm[i]['linkend'],
thickness=20,
rgba=lftrgba)
i = hrp5robot.lftarm[i]['child']
def checkMouse2Drag(self):
curm2pos = self.getMouse2Aim()
if curm2pos is None:
if self.lastm2pos is not None:
self.lastm2pos = None
return
if self.lastm2pos is None:
# first time click
self.lastm2pos = curm2pos
return
relm2vec = curm2pos - self.lastm2pos
if relm2vec.length() > 5:
tmplookatp = self.lookatp-relm2vec
if (tmplookatp[0] > -1500 and tmplookatp[0] < 1500) and \
(tmplookatp[1] > -1500 and tmplookatp[1] < 1500) and \
(tmplookatp[2] > -1500 and tmplookatp[2] < 1500):
self.lookatp = tmplookatp
self.pandabase.cam.setPos(self.pandabase.cam.getPos()-relm2vec)
self.pandabase.cam.lookAt(self.lookatp)
self.last2mpos = self.getMouse2Aim()
if self.rotatecenternp is not None:
self.rotatecenternp.detachNode()
self.rotatecenternp = pg.plotDumbbell(self.pandabase.render, \
np.array([self.lookatp[0], self.lookatp[1], self.lookatp[2]]), \
np.array([self.lookatp[0], self.lookatp[1], self.lookatp[2]]), \
thickness=10, rgba = np.array([1,1,0,0]), plotname = "transcenter")
def plotstick(pandanp, hrp5robot, rgtrbga=np.array([.5 ,0 ,0 ,0]), lftrgba=np.array([.5 ,0 ,0 ,0])):
"""
plot the stick model of the nextage robot in panda3d
:param pandanp: a panda3d nodepath
:param hrp5robot: the Hrp5Robot object, see Hrp5robot.py
:param rgtrbga: color of right arm
:param lftrgba: color of left arm
:return: null
author: weiwei
date: 20161202
"""
i = 0
while i != -1:
pg.plotDumbbell(pandanp, spos=hrp5robot.rgtarm[i]['linkpos'], epos=hrp5robot.rgtarm[i]['linkend'],
thickness=20, rgba=rgtrbga)
i = hrp5robot.rgtarm[i]['child']
i = 0
while i != -1:
pg.plotDumbbell(pandanp, spos=hrp5robot.lftarm[i]['linkpos'], epos=hrp5robot.lftarm[i]['linkend'],
thickness=20, rgba=lftrgba)
i = hrp5robot.lftarm[i]['child']
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
hrp5mnp = hrp5plot.genHrp5mnp(hrp5robot)
# hrp5mnp.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.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 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