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.02
updateRbdVW(rbd, dtime)
model = loader.loadModel(model_filepath)
model.reparentTo(base.render)
model.setMat(pg.cvtMat4(rbd.rotmat))
model.setPos(pg.npToV3(rbd.pos))
return task.again
def updateshow(rbd, rbdnp, framenp, task):
for frame in framenp:
frame.detachNode()
dtime = 0.02
updateRbdVW(rbd, dtime)
model = loader.loadModel(model_filepath)
model.reparentTo(base.render)
model.setMat(pg.cvtMat4(rbd.rotmat))
model.setPos(pg.npToV3(rbd.pos))
return task.again
def gettcpframe(self, armid = 'rgt'):
"""
get the local frame of tcp in Mat4 format
:param armid:
author: weiwei
date: 20170412
"""
armlj = self.rgtarm
if armid == "lft":
armlj = self.lftarm
return pg.cvtMat4(armlj[-1]['rotmat'], armlj[-1]['linkpos'])
def gettcpframe(self, armname='rgt'):
"""
get the local frame of tcp in Mat4 format
:param armname:
author: weiwei
date: 20170412
"""
armlj = self.rgtarm
if armname == "lft":
armlj = self.lftarm
return pg.cvtMat4(armlj[-1]['rotmat'], armlj[-1]['linkpos'])
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
def genmnp(nxtrobot, handpkg, jawwidthrgt = None, jawwidthlft = None):
"""
generate a panda3d nodepath for the nxtrobot
mnp indicates this function generates a mesh nodepath
:param nxtrobot: the NxtRobot object, see nextage.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20161109
"""
nxtmnp = NodePath("nxtmnp")
this_dir, this_filename = os.path.split(__file__)
# mainbody, waist, body, head (neck is not plotted)
nxtwaist_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_waist.egg"))
nxtbody_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_body.egg"))
nxthead_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_head.egg"))
nxtwaist_model = loader.loadModel(nxtwaist_filepath)
nxtbody_model = loader.loadModel(nxtbody_filepath)
nxthead_model = loader.loadModel(nxthead_filepath)
nxtwaist_nodepath = NodePath("nxtwaist")
nxtbody_nodepath = NodePath("nxtbody")
nxthead_nodepath = NodePath("nxthead")
nxtwaist_model.instanceTo(nxtwaist_nodepath)
nxtwaist_rotmat = pandageom.cvtMat4(nxtrobot.base['rotmat'])
nxtwaist_nodepath.setMat(nxtwaist_rotmat)
nxtbody_model.instanceTo(nxtbody_nodepath)
nxthead_model.instanceTo(nxthead_nodepath)
nxthead_nodepath.setH(nxtrobot.initjnts[1])
nxthead_nodepath.setP(nxtrobot.initjnts[2])
nxthead_nodepath.setZ(569.5)
nxtwaist_nodepath.reparentTo(nxtmnp)
nxtbody_nodepath.reparentTo(nxtwaist_nodepath)
nxthead_nodepath.reparentTo(nxtwaist_nodepath)
# rgtarm
nxtrgtarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj0.egg"))
nxtrgtarmlj0_model = loader.loadModel(nxtrgtarmlj0_filepath)
nxtrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath")
nxtrgtarmlj0_model.instanceTo(nxtrgtarmlj0_nodepath)
nxtrgtarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[1]['rotmat'], nxtrobot.rgtarm[1]['linkpos'])
nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat)
nxtrgtarmlj0_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj1.egg"))
nxtrgtarmlj1_model = loader.loadModel(nxtrgtarmlj1_filepath)
nxtrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath")
nxtrgtarmlj1_model.instanceTo(nxtrgtarmlj1_nodepath)
nxtrgtarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[2]['rotmat'], nxtrobot.rgtarm[2]['linkpos'])
nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat)
nxtrgtarmlj1_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj2.egg"))
nxtrgtarmlj2_model = loader.loadModel(nxtrgtarmlj2_filepath)
nxtrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath")
nxtrgtarmlj2_model.instanceTo(nxtrgtarmlj2_nodepath)
nxtrgtarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[3]['rotmat'], nxtrobot.rgtarm[3]['linkpos'])
nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat)
nxtrgtarmlj2_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj3.egg"))
nxtrgtarmlj3_model = loader.loadModel(nxtrgtarmlj3_filepath)
nxtrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath")
nxtrgtarmlj3_model.instanceTo(nxtrgtarmlj3_nodepath)
nxtrgtarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[4]['rotmat'], nxtrobot.rgtarm[4]['linkpos'])
nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat)
nxtrgtarmlj3_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj4.egg"))
nxtrgtarmlj4_model = loader.loadModel(nxtrgtarmlj4_filepath)
nxtrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath")
nxtrgtarmlj4_model.instanceTo(nxtrgtarmlj4_nodepath)
nxtrgtarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[5]['rotmat'], nxtrobot.rgtarm[5]['linkpos'])
nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat)
nxtrgtarmlj4_nodepath.reparentTo(nxtmnp)
# lftarm
nxtlftarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj0.egg"))
nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath)
nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath")
nxtlftarmlj0_model.instanceTo(nxtlftarmlj0_nodepath)
nxtlftarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos'])
nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat)
nxtlftarmlj0_nodepath.reparentTo(nxtmnp)
nxtlftarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj1.egg"))
nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath)
nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath")
nxtlftarmlj1_model.instanceTo(nxtlftarmlj1_nodepath)
nxtlftarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos'])
nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat)
nxtlftarmlj1_nodepath.reparentTo(nxtmnp)
nxtlftarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj2.egg"))
nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath)
nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath")
nxtlftarmlj2_model.instanceTo(nxtlftarmlj2_nodepath)
nxtlftarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos'])
nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat)
nxtlftarmlj2_nodepath.reparentTo(nxtmnp)
nxtlftarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj3.egg"))
nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath)
nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath")
nxtlftarmlj3_model.instanceTo(nxtlftarmlj3_nodepath)
nxtlftarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos'])
nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat)
nxtlftarmlj3_nodepath.reparentTo(nxtmnp)
nxtlftarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj4.egg"))
nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath)
nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath")
nxtlftarmlj4_model.instanceTo(nxtlftarmlj4_nodepath)
nxtlftarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos'])
nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat)
nxtlftarmlj4_nodepath.reparentTo(nxtmnp)
# rgthnd
nxtrgthnd = handpkg.newHand('rgt')
nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos'])
nxtrgthnd.setMat(nxtlftarmlj5_rotmat)
nxtrgthnd.reparentTo(nxtmnp)
if jawwidthrgt is not None:
nxtrgthnd.setJawwidth(jawwidthrgt)
# lfthnd
nxtlfthnd = handpkg.newHand('lft')
nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[6]['rotmat'], nxtrobot.lftarm[6]['linkpos'])
nxtlfthnd.setMat(nxtlftarmlj5_rotmat)
nxtlfthnd.reparentTo(nxtmnp)
if jawwidthlft is not None:
nxtlfthnd.setJawwidth(jawwidthlft)
return nxtmnp
from panda3d.core import *
import pandaplotutils.pandageom as pg
import pandaplotutils.pandactrl as pc
base = pc.World(camp = [1000,0,0], lookatp = [0,0,0])
rbd = Rigidbody(mass = 10.0, pos = np.array([0,0,0.0]), com = [0.0,0.0,0.0],
rotmat = rm.rodrigues(np.array([1,0,0]), 1.8),
inertiatensor = np.array([[1732.0,0.0,0.0],[0.0,1732.0,0.0],[0.0,0.0,3393.0]]))
rbd.linearv = np.array([0,0,0])
rbd.angularw = np.array([0,0,50])
this_dir, this_filename = os.path.split(__file__)
model_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "models", "top.egg"))
model = loader.loadModel(model_filepath)
model.reparentTo(base.render)
model.setMat(pg.cvtMat4(rbd.rotmat))
model.setPos(pg.npToV3(rbd.pos))
# base = pc.World(camp=[3000, 0, 3000], lookatp=[0, 0, 0])
#
# rbd = Rigidbody(mass = 1.0, pos=np.array([0, 0, 0.0]), rotmat=np.identity(3),
# inertiatensor=np.array([[80833.3, 0.0, 0.0], [0.0, 68333.3, 0.0], [0.0, 0.0, 14166.7]]))
# rbd.linearv = np.array([0,0,0])
# rbd.angularw = np.array([1, 1, 1])
#
# this_dir, this_filename = os.path.split(__file__)
# model_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "models", "box.egg"))
# model = loader.loadModel(model_filepath)
# model.reparentTo(base.render)
def genHrp5mnp(hrp5robot, jawwidthrgt=None, jawwidthlft=None):
"""
generate a panda3d nodepath for the hrp5robot
mnp indicates this function generates a mesh nodepath
:param hrp5robot: the Hrp5Robot object, see hrp5.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20161202
"""
hrp5mnp = NodePath("hrp5mnp")
this_dir, this_filename = os.path.split(__file__)
# chest0-2, head1 (neck is not plotted)
hrp5chest0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5egg", "hrp5_chest0.egg"))
hrp5chest1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5egg", "hrp5_chest1.egg"))
hrp5chest2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5egg", "hrp5_chest2.egg"))
hrp5head1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5egg", "hrp5_head1.egg"))
hrp5chest0_model = loader.loadModel(hrp5chest0_filepath)
hrp5chest1_model = loader.loadModel(hrp5chest1_filepath)
hrp5chest2_model = loader.loadModel(hrp5chest2_filepath)
hrp5head1_model = loader.loadModel(hrp5head1_filepath)
hrp5chest0_nodepath = NodePath("hrp5chest0")
hrp5chest1_nodepath = NodePath("hrp5chest1")
hrp5chest2_nodepath = NodePath("hrp5chest2")
hrp5head1_nodepath = NodePath("hrp5head1")
hrp5chest0_model.instanceTo(hrp5chest0_nodepath)
hrp5chest0_rotmat = pg.cvtMat4(hrp5robot.base['rotmat'])
hrp5chest0_nodepath.setMat(hrp5chest0_rotmat)
hrp5chest0_nodepath.setZ(223)
hrp5chest1_model.instanceTo(hrp5chest1_nodepath)
hrp5chest1_nodepath.setColor(.7, .7, .2, 1)
hrp5chest2_model.instanceTo(hrp5chest2_nodepath)
hrp5chest2_nodepath.setX(-68)
hrp5chest2_nodepath.setColor(.7, .7, .7, 1)
hrp5head1_model.instanceTo(hrp5head1_nodepath)
hrp5head1_nodepath.setH(hrp5robot.initjnts[0])
hrp5head1_nodepath.setP(hrp5robot.initjnts[1])
hrp5head1_nodepath.setX(43)
hrp5head1_nodepath.setZ(440)
hrp5head1_nodepath.setColor(.5, .5, .5, 1)
hrp5chest0_nodepath.reparentTo(hrp5mnp)
hrp5chest1_nodepath.reparentTo(hrp5chest0_nodepath)
hrp5chest2_nodepath.reparentTo(hrp5chest0_nodepath)
hrp5head1_nodepath.reparentTo(hrp5chest0_nodepath)
# rgtarm
hrp5rgtarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5egg", "hrp5_rscap.egg"))
hrp5rgtarmlj0_model = loader.loadModel(hrp5rgtarmlj0_filepath)
hrp5rgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath")
hrp5rgtarmlj0_model.instanceTo(hrp5rgtarmlj0_nodepath)
hrp5rgtarmlj0_rotmat = pg.cvtMat4(hrp5robot.rgtarm[1]['rotmat'],
hrp5robot.rgtarm[1]['linkpos'])
hrp5rgtarmlj0_nodepath.setMat(hrp5rgtarmlj0_rotmat)
hrp5rgtarmlj0_nodepath.setColor(.5, .5, .5, 1)
hrp5rgtarmlj0_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5egg", "hrp5_rlink0.egg"))
hrp5rgtarmlj1_model = loader.loadModel(hrp5rgtarmlj1_filepath)
hrp5rgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath")
hrp5rgtarmlj1_model.instanceTo(hrp5rgtarmlj1_nodepath)
hrp5rgtarmlj1_rotmat = pg.cvtMat4(hrp5robot.rgtarm[2]['rotmat'],
hrp5robot.rgtarm[2]['linkpos'])
hrp5rgtarmlj1_nodepath.setMat(hrp5rgtarmlj1_rotmat)
hrp5rgtarmlj1_nodepath.setColor(.5, .5, .5, 1)
hrp5rgtarmlj1_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5egg", "hrp5_rlink1.egg"))
hrp5rgtarmlj2_model = loader.loadModel(hrp5rgtarmlj2_filepath)
hrp5rgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath")
hrp5rgtarmlj2_model.instanceTo(hrp5rgtarmlj2_nodepath)
hrp5rgtarmlj2_rotmat = pg.cvtMat4(hrp5robot.rgtarm[3]['rotmat'],
hrp5robot.rgtarm[3]['linkpos'])
hrp5rgtarmlj2_nodepath.setMat(hrp5rgtarmlj2_rotmat)
hrp5rgtarmlj2_nodepath.setColor(.5, .5, .5, 1)
hrp5rgtarmlj2_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5egg", "hrp5_rlink2.egg"))
hrp5rgtarmlj3_model = loader.loadModel(hrp5rgtarmlj3_filepath)
hrp5rgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath")
hrp5rgtarmlj3_model.instanceTo(hrp5rgtarmlj3_nodepath)
hrp5rgtarmlj3_rotmat = pg.cvtMat4(hrp5robot.rgtarm[4]['rotmat'],
hrp5robot.rgtarm[4]['linkpos'])
hrp5rgtarmlj3_nodepath.setMat(hrp5rgtarmlj3_rotmat)
hrp5rgtarmlj3_nodepath.setColor(.5, .5, .5, 1)
hrp5rgtarmlj3_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5egg", "hrp5_rlink3.egg"))
hrp5rgtarmlj4_model = loader.loadModel(hrp5rgtarmlj4_filepath)
hrp5rgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath")
hrp5rgtarmlj4_model.instanceTo(hrp5rgtarmlj4_nodepath)
hrp5rgtarmlj4_rotmat = pg.cvtMat4(hrp5robot.rgtarm[5]['rotmat'],
hrp5robot.rgtarm[5]['linkpos'])
hrp5rgtarmlj4_nodepath.setMat(hrp5rgtarmlj4_rotmat)
hrp5rgtarmlj4_nodepath.setColor(.5, .5, .5, 1)
hrp5rgtarmlj4_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj5_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5egg", "hrp5_rlink4.egg"))
hrp5rgtarmlj5_model = loader.loadModel(hrp5rgtarmlj5_filepath)
hrp5rgtarmlj5_nodepath = NodePath("nxtrgtarmlj5_nodepath")
hrp5rgtarmlj5_model.instanceTo(hrp5rgtarmlj5_nodepath)
hrp5rgtarmlj5_rotmat = pg.cvtMat4(hrp5robot.rgtarm[6]['rotmat'],
hrp5robot.rgtarm[6]['linkpos'])
hrp5rgtarmlj5_nodepath.setMat(hrp5rgtarmlj5_rotmat)
hrp5rgtarmlj5_nodepath.setColor(.5, .5, .5, 1)
hrp5rgtarmlj5_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj6_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5egg", "hrp5_rlink5.egg"))
hrp5rgtarmlj6_model = loader.loadModel(hrp5rgtarmlj6_filepath)
hrp5rgtarmlj6_nodepath = NodePath("nxtrgtarmlj6_nodepath")
hrp5rgtarmlj6_model.instanceTo(hrp5rgtarmlj6_nodepath)
hrp5rgtarmlj6_rotmat = pg.cvtMat4(hrp5robot.rgtarm[7]['rotmat'],
hrp5robot.rgtarm[7]['linkpos'])
hrp5rgtarmlj6_nodepath.setMat(hrp5rgtarmlj6_rotmat)
hrp5rgtarmlj6_nodepath.setColor(.5, .5, .5, 1)
hrp5rgtarmlj6_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj7_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5egg", "hrp5_rlink6.egg"))
hrp5rgtarmlj7_model = loader.loadModel(hrp5rgtarmlj7_filepath)
hrp5rgtarmlj7_nodepath = NodePath("nxtrgtarmlj7_nodepath")
hrp5rgtarmlj7_model.instanceTo(hrp5rgtarmlj7_nodepath)
hrp5rgtarmlj7_rotmat = pg.cvtMat4(hrp5robot.rgtarm[8]['rotmat'],
hrp5robot.rgtarm[8]['linkpos'])
hrp5rgtarmlj7_nodepath.setMat(hrp5rgtarmlj7_rotmat)
hrp5rgtarmlj7_nodepath.setColor(.5, .5, .5, 1)
hrp5rgtarmlj7_nodepath.reparentTo(hrp5mnp)
#
# # lftarm
# nxtlftarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj0.egg"))
# nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath)
# nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath")
# nxtlftarmlj0_model.instanceTo(nxtlftarmlj0_nodepath)
# nxtlftarmlj0_rotmat = pg.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos'])
# nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat)
# nxtlftarmlj0_nodepath.reparentTo(nxtmnp)
#
# nxtlftarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj1.egg"))
# nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath)
# nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath")
# nxtlftarmlj1_model.instanceTo(nxtlftarmlj1_nodepath)
# nxtlftarmlj1_rotmat = pg.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos'])
# nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat)
# nxtlftarmlj1_nodepath.reparentTo(nxtmnp)
#
# nxtlftarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj2.egg"))
# nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath)
# nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath")
# nxtlftarmlj2_model.instanceTo(nxtlftarmlj2_nodepath)
# nxtlftarmlj2_rotmat = pg.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos'])
# nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat)
# nxtlftarmlj2_nodepath.reparentTo(nxtmnp)
#
# nxtlftarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj3.egg"))
# nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath)
# nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath")
# nxtlftarmlj3_model.instanceTo(nxtlftarmlj3_nodepath)
# nxtlftarmlj3_rotmat = pg.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos'])
# nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat)
# nxtlftarmlj3_nodepath.reparentTo(nxtmnp)
#
# nxtlftarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj4.egg"))
# nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath)
# nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath")
# nxtlftarmlj4_model.instanceTo(nxtlftarmlj4_nodepath)
# nxtlftarmlj4_rotmat = pg.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos'])
# nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat)
# nxtlftarmlj4_nodepath.reparentTo(nxtmnp)
# rgthnd
hrp5robotrgthnd = rtq85.Rtq85()
hrp5robotrgtarmlj9_rotmat = pg.cvtMat4(hrp5robot.rgtarm[9]['rotmat'],
hrp5robot.rgtarm[9]['linkpos'])
# pg.plotAxisSelf(hrp5mnp, hrp5robot.rgtarm[9]['linkend'], hrp5robotrgtarmlj9_rotmat)
hrp5robotrgthnd.setMat(hrp5robotrgtarmlj9_rotmat)
hrp5robotrgthnd.reparentTo(hrp5mnp)
if jawwidthrgt is not None:
hrp5robotrgthnd.setJawwidth(jawwidthrgt)
# lfthnd
# hrp5robotlfthnd = rtq85.Rtq85()
# hrp5robotlftarmlj9_rotmat = pg.cvtMat4(hrp5robot.lftarm[9]['rotmat'], hrp5robot.lftarm[9]['linkpos'])
# pg.plotAxisSelf(hrp5mnp, hrp5robot.lftarm[9]['linkend'], hrp5robotlftarmlj9_rotmat)
# hrp5robotlfthnd.setMat(hrp5robotlftarmlj9_rotmat)
# hrp5robotlfthnd.reparentTo(hrp5mnp)
# if jawwidthlft is not None:
# hrp5robotlfthnd.setJawwidth(jawwidthlft)
return hrp5mnp
def genmnp(self,
robot,
jawwidthrgt=0,
jawwidthlft=0,
toggleendcoord=True,
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)
identmat4 = Mat4.identMat()
# body
nxtwaist_rotmat = pg.cvtMat4(robot.base['rotmat'])
self.__nxtwaist_nodepath.setMat(nxtwaist_rotmat)
# rgtarm
nxtrgtarmlj0_rotmat = pg.cvtMat4(robot.rgtarm[1]['rotmat'],
robot.rgtarm[1]['linkpos'])
self.__nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat)
nxtrgtarmlj1_rotmat = pg.cvtMat4(robot.rgtarm[2]['rotmat'],
robot.rgtarm[2]['linkpos'])
self.__nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat)
nxtrgtarmlj2_rotmat = pg.cvtMat4(robot.rgtarm[3]['rotmat'],
robot.rgtarm[3]['linkpos'])
self.__nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat)
nxtrgtarmlj3_rotmat = pg.cvtMat4(robot.rgtarm[4]['rotmat'],
robot.rgtarm[4]['linkpos'])
self.__nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat)
nxtrgtarmlj4_rotmat = pg.cvtMat4(robot.rgtarm[5]['rotmat'],
robot.rgtarm[5]['linkpos'])
self.__nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat)
# lftarm
nxtlftarmlj0_rotmat = pg.cvtMat4(robot.lftarm[1]['rotmat'],
robot.lftarm[1]['linkpos'])
self.__nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat)
nxtlftarmlj1_rotmat = pg.cvtMat4(robot.lftarm[2]['rotmat'],
robot.lftarm[2]['linkpos'])
self.__nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat)
nxtlftarmlj2_rotmat = pg.cvtMat4(robot.lftarm[3]['rotmat'],
robot.lftarm[3]['linkpos'])
self.__nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat)
nxtlftarmlj3_rotmat = pg.cvtMat4(robot.lftarm[4]['rotmat'],
robot.lftarm[4]['linkpos'])
self.__nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat)
nxtlftarmlj4_rotmat = pg.cvtMat4(robot.lftarm[5]['rotmat'],
robot.lftarm[5]['linkpos'])
self.__nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat)
self.__nxtwaist_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)
# rgthand
nxtrgtarmlj5_rotmat = pg.cvtMat4(robot.rgtarm[6]['rotmat'],
robot.rgtarm[6]['linkpos'])
if self.rgthnd is not None:
self.rgthnd.setMat(nxtrgtarmlj5_rotmat)
self.rgthnd.setJawwidth(jawwidthrgt)
self.rgthnd.reparentTo(robotmesh_nodepath)
# lfthand
nxtlftarmlj5_rotmat = pg.cvtMat4(robot.lftarm[6]['rotmat'],
robot.lftarm[6]['linkpos'])
if self.lfthnd is not None:
self.lfthnd.setMat(nxtlftarmlj5_rotmat)
self.lfthnd.setJawwidth(jawwidthlft)
self.lfthnd.reparentTo(robotmesh_nodepath)
# endcoord
if toggleendcoord:
self.pggen.plotAxis(robotmesh_nodepath,
spos=robot.lftarm[6]['linkend'],
pandamat4=nxtlftarmlj5_rotmat)
self.pggen.plotAxis(robotmesh_nodepath,
spos=robot.rgtarm[6]['linkend'],
pandamat4=nxtrgtarmlj5_rotmat)
# toggle all coord
if togglejntscoord:
for i in range(1, 7):
self.pggen.plotAxis(robotmesh_nodepath,
spos=robot.lftarm[i]['linkpos'],
pandamat4=pg.cvtMat4(
robot.lftarm[i - 1]['rotmat'],
robot.lftarm[i]['linkpos']))
return copy.deepcopy(robotmesh_nodepath)
def removeHndccShow(self, base, discretesize=8):
"""
Handcc means hand collision detection
This one is developed for demonstration
This function should be called after executing removeHndcc
:param discretesize: the number of hand orientations
:return: delayTime
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
if self.rtq85plotlist:
for rtq85plotnode in self.rtq85plotlist:
rtq85plotnode.removeNode()
self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
if self.counter2 == 0:
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
self.counter2 += 1
if self.counter2 >= discretesize:
self.counter2 = 0
print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs_precc[self.counter]):
if j == 0:
print j, contactpair
# for angleid in range(discretesize):
angleid = self.counter2
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[self.counter][j][0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
# TODO setting jawwidth to 80 is enough
# since fgrpcc already detects inner collisions
tmprtq85.setJawwidth(fgrdist)
tmprtq85.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmprtq85.setMat(pandageom.cvtMat4(rotmat) * tmprtq85.getMat())
axx = tmprtq85.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]])
tmprtq85.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
self.hndbullnode = cd.genCollisionMeshMultiNp(tmprtq85.rtq85np, base.render)
result = self.bulletworld.contactTest(self.hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmprtq85.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(self.gripcontactpairnormals_precc[self.counter][j])
# pandageom.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pandageom.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
tmprtq85.setColor([1, 1, 1, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
else:
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))
tmprtq85.setColor([.5, 0, 0, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
def genmnp_list(self, hrp5nrobot, jawwidthrgt = None, jawwidthlft = None):
"""
generate a panda3d nodepath for the hrp5robot
mnp indicates this function generates a mesh nodepath
return [[right arm mnp list], [leftarm mnp list], [body mnp list]]
where one arm mnp list is ordered from base to end-effector
this function is designed for collision detection
:param hrp5robot: the Hrp5Robot object, see hrp5.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20170613
"""
identmat4 = Mat4.identMat()
# body
self.__hrp5nbody_nodepath.setMat(identmat4)
self.__hrp5nbody_nodepath.setZ(self.__hrp5nmnp, 0)
# chest 0
self.__hrp5nchest0_nodepath.setMat(identmat4)
self.__hrp5nchest0_nodepath.setZ(self.__hrp5nmnp, 274)
# chest 1
self.__hrp5nchest1_nodepath.setMat(identmat4)
self.__hrp5nchest1_nodepath.setColor(.7,.7,.2,1)
# chest 2
hrp5nchest2_rotmat = pg.cvtMat4(hrp5nrobot.base['rotmat'])
self.__hrp5nchest2_nodepath.setMat(hrp5nchest2_rotmat)
self.__hrp5nchest2_nodepath.setColor(.7,.7,.7,1)
# head 0
self.__hrp5nhead0_nodepath.setMat(identmat4)
self.__hrp5nhead0_nodepath.setH(hrp5nrobot.initjnts[0])
self.__hrp5nhead0_nodepath.setX(32)
self.__hrp5nhead0_nodepath.setZ(521)
# head 1
self.__hrp5nhead1_nodepath.setP(hrp5nrobot.initjnts[1])
self.__hrp5nhead1_nodepath.setX(40)
self.__hrp5nhead1_nodepath.setColor(.5,.5,.5,1)
# rgtarm 0
hrp5nrgtarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[1]['rotmat'], hrp5nrobot.rgtarm[1]['linkpos'])
self.__hrp5nrgtarmlj0_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj0_rotmat)
self.__hrp5nrgtarmlj0_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 1
hrp5nrgtarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[2]['rotmat'], hrp5nrobot.rgtarm[2]['linkpos'])
self.__hrp5nrgtarmlj1_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj1_rotmat)
self.__hrp5nrgtarmlj1_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 2
hrp5nrgtarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[3]['rotmat'], hrp5nrobot.rgtarm[3]['linkpos'])
self.__hrp5nrgtarmlj2_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj2_rotmat)
self.__hrp5nrgtarmlj2_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 3
hrp5nrgtarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[4]['rotmat'], hrp5nrobot.rgtarm[4]['linkpos'])
self.__hrp5nrgtarmlj3_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj3_rotmat)
self.__hrp5nrgtarmlj3_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 4
hrp5nrgtarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[5]['rotmat'], hrp5nrobot.rgtarm[5]['linkpos'])
self.__hrp5nrgtarmlj4_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj4_rotmat)
self.__hrp5nrgtarmlj4_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 5
hrp5nrgtarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[6]['rotmat'], hrp5nrobot.rgtarm[6]['linkpos'])
self.__hrp5nrgtarmlj5_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj5_rotmat)
self.__hrp5nrgtarmlj5_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 6
hrp5nrgtarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[7]['rotmat'], hrp5nrobot.rgtarm[7]['linkpos'])
self.__hrp5nrgtarmlj6_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj6_rotmat)
self.__hrp5nrgtarmlj6_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 7
hrp5nrgtarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[8]['rotmat'], hrp5nrobot.rgtarm[8]['linkpos'])
self.__hrp5nrgtarmlj7_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj7_rotmat)
self.__hrp5nrgtarmlj7_nodepath.setColor(.5, .5, .5, 1)
# lftarm 0
hrp5nlftarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[1]['rotmat'], hrp5nrobot.lftarm[1]['linkpos'])
self.__hrp5nlftarmlj0_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj0_rotmat)
self.__hrp5nlftarmlj0_nodepath.setColor(.5, .5, .5, 1)
# lftarm 1
hrp5nlftarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[2]['rotmat'], hrp5nrobot.lftarm[2]['linkpos'])
self.__hrp5nlftarmlj1_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj1_rotmat)
self.__hrp5nlftarmlj1_nodepath.setColor(.5, .5, .5, 1)
# lftarm 2
hrp5nlftarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[3]['rotmat'], hrp5nrobot.lftarm[3]['linkpos'])
self.__hrp5nlftarmlj2_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj2_rotmat)
self.__hrp5nlftarmlj2_nodepath.setColor(.5, .5, .5, 1)
# lftarm 3
hrp5nlftarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[4]['rotmat'], hrp5nrobot.lftarm[4]['linkpos'])
self.__hrp5nlftarmlj3_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj3_rotmat)
self.__hrp5nlftarmlj3_nodepath.setColor(.5, .5, .5, 1)
# lftarm 4
hrp5nlftarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[5]['rotmat'], hrp5nrobot.lftarm[5]['linkpos'])
self.__hrp5nlftarmlj4_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj4_rotmat)
self.__hrp5nlftarmlj4_nodepath.setColor(.5, .5, .5, 1)
# lftarm 5
hrp5nlftarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[6]['rotmat'], hrp5nrobot.lftarm[6]['linkpos'])
self.__hrp5nlftarmlj5_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj5_rotmat)
self.__hrp5nlftarmlj5_nodepath.setColor(.5, .5, .5, 1)
# lftarm 6
hrp5nlftarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[7]['rotmat'], hrp5nrobot.lftarm[7]['linkpos'])
self.__hrp5nlftarmlj6_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj6_rotmat)
self.__hrp5nlftarmlj6_nodepath.setColor(.5, .5, .5, 1)
# lftarm 7
hrp5nlftarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[8]['rotmat'], hrp5nrobot.lftarm[8]['linkpos'])
self.__hrp5nlftarmlj7_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj7_rotmat)
self.__hrp5nlftarmlj7_nodepath.setColor(.5, .5, .5, 1)
# rgthand
hrp5nrobotrgtarmlj9_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[9]['rotmat'], hrp5nrobot.rgtarm[9]['linkpos'])
self.hrp5nrobotrgthnd.setMat(self.__hrp5nmnp, hrp5nrobotrgtarmlj9_rotmat)
if jawwidthrgt is not None:
self.hrp5nrobotrgthnd.setJawwidth(jawwidthrgt)
# lfthand
hrp5nrobotlftarmlj9_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[9]['rotmat'], hrp5nrobot.lftarm[9]['linkpos'])
self.hrp5nrobotlfthnd.setMat(self.__hrp5nmnp, hrp5nrobotlftarmlj9_rotmat)
if jawwidthlft is not None:
self.hrp5nrobotlfthnd.setJawwidth(jawwidthlft)
return copy.deepcopy([[self.__hrp5nrgtarmlj0_nodepath, self.__hrp5nrgtarmlj1_nodepath, self.__hrp5nrgtarmlj2_nodepath,
self.__hrp5nrgtarmlj3_nodepath, self.__hrp5nrgtarmlj4_nodepath, self.__hrp5nrgtarmlj5_nodepath,
self.__hrp5nrgtarmlj6_nodepath, self.__hrp5nrgtarmlj7_nodepath, self.hrp5nrobotrgthnd.handnp],
[self.__hrp5nlftarmlj0_nodepath, self.__hrp5nlftarmlj1_nodepath, self.__hrp5nlftarmlj2_nodepath,
self.__hrp5nlftarmlj3_nodepath, self.__hrp5nlftarmlj4_nodepath, self.__hrp5nlftarmlj5_nodepath,
self.__hrp5nlftarmlj6_nodepath, self.__hrp5nlftarmlj7_nodepath, self.hrp5nrobotlfthnd.handnp],
[self.__hrp5nbody_nodepath]])
def genmnp(self, nxtrobot, jawwidthrgt=None, jawwidthlft=None):
"""
generate a panda3d nodepath for the nxtrobot
mnp indicates this function generates a mesh nodepath
:param nxtrobot: the nxtrobot object, see nxtrobot.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20180109
"""
identmat4 = Mat4.identMat()
# body
nxtwaist_rotmat = pg.cvtMat4(nxtrobot.base['rotmat'])
self.__nxtwaist_nodepath.setMat(nxtwaist_rotmat)
# rgtarm
nxtrgtarmlj0_rotmat = pg.cvtMat4(nxtrobot.rgtarm[1]['rotmat'],
nxtrobot.rgtarm[1]['linkpos'])
self.__nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat)
nxtrgtarmlj1_rotmat = pg.cvtMat4(nxtrobot.rgtarm[2]['rotmat'],
nxtrobot.rgtarm[2]['linkpos'])
self.__nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat)
nxtrgtarmlj2_rotmat = pg.cvtMat4(nxtrobot.rgtarm[3]['rotmat'],
nxtrobot.rgtarm[3]['linkpos'])
self.__nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat)
nxtrgtarmlj3_rotmat = pg.cvtMat4(nxtrobot.rgtarm[4]['rotmat'],
nxtrobot.rgtarm[4]['linkpos'])
self.__nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat)
nxtrgtarmlj4_rotmat = pg.cvtMat4(nxtrobot.rgtarm[5]['rotmat'],
nxtrobot.rgtarm[5]['linkpos'])
self.__nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat)
# lftarm
nxtlftarmlj0_rotmat = pg.cvtMat4(nxtrobot.lftarm[1]['rotmat'],
nxtrobot.lftarm[1]['linkpos'])
self.__nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat)
nxtlftarmlj1_rotmat = pg.cvtMat4(nxtrobot.lftarm[2]['rotmat'],
nxtrobot.lftarm[2]['linkpos'])
self.__nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat)
nxtlftarmlj2_rotmat = pg.cvtMat4(nxtrobot.lftarm[3]['rotmat'],
nxtrobot.lftarm[3]['linkpos'])
self.__nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat)
nxtlftarmlj3_rotmat = pg.cvtMat4(nxtrobot.lftarm[4]['rotmat'],
nxtrobot.lftarm[4]['linkpos'])
self.__nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat)
nxtlftarmlj4_rotmat = pg.cvtMat4(nxtrobot.lftarm[5]['rotmat'],
nxtrobot.lftarm[5]['linkpos'])
self.__nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat)
# rgthand
nxtlftarmlj5_rotmat = pg.cvtMat4(nxtrobot.rgtarm[6]['rotmat'],
nxtrobot.rgtarm[6]['linkpos'])
self.nxtrgthnd.setMat(nxtlftarmlj5_rotmat)
pg.plotAxisSelf(self.__nxtmnp, nxtrobot.rgtarm[6]['linkend'],
nxtlftarmlj5_rotmat)
if jawwidthrgt is not None:
self.nxtrgthnd.setJawwidth(jawwidthrgt)
# lfthand
nxtlftarmlj5_rotmat = pg.cvtMat4(nxtrobot.lftarm[6]['rotmat'],
nxtrobot.lftarm[6]['linkpos'])
self.nxtlfthnd.setMat(nxtlftarmlj5_rotmat)
pg.plotAxisSelf(self.__nxtmnp, nxtrobot.lftarm[6]['linkend'],
nxtlftarmlj5_rotmat)
if jawwidthlft is not None:
self.nxtlfthnd.setJawwidth(jawwidthlft)
return copy.deepcopy(self.__nxtmnp)
def __init__(self, jawwidth=85):
'''
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)
## input
pandabase:
the showbase() object
jawwidth:
the distance between fingertips
## output
rtq85np:
the nodepath of this rtq85 hand
author: weiwei
date: 20160627
'''
self.rtq85np = NodePath("rtq85hnd")
self.handnp = self.rtq85np
self.jawwidth = jawwidth
this_dir, this_filename = os.path.split(__file__)
rtq85basepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg", "robotiq_85_base_link.egg"))
rtq85fingerpath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg", "robotiq_85_finger_link.egg"))
rtq85fingertippath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg", "robotiq_85_finger_tip_link.egg"))
rtq85innerknucklepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg", "robotiq_85_inner_knuckle_link.egg"))
rtq85knucklepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg", "robotiq_85_knuckle_link.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)
rtq85base.setPos(0,0,0)
# left and right outer knuckle
rtq85_knucklel.instanceTo(rtq85lknuckle)
rtq85lknuckle.setPos(-30.60114443, 54.90451627, 0)
rtq85lknuckle.setHpr(0, 0, 180)
rtq85lknuckle.reparentTo(rtq85base)
rtq85_knucklel.instanceTo(rtq85rknuckle)
rtq85rknuckle.setPos(30.60114443, 54.90451627, 0)
rtq85rknuckle.setHpr(0, 0, 0)
rtq85rknuckle.reparentTo(rtq85base)
# left and right finger
rtq85_fingerl.instanceTo(rtq85lfgr)
rtq85lfgr.setPos(31.48504435, -4.08552455, 0)
rtq85lfgr.reparentTo(rtq85lknuckle)
rtq85_fingerl.instanceTo(rtq85rfgr)
rtq85rfgr.setPos(31.48504435, -4.08552455, 0)
rtq85rfgr.reparentTo(rtq85rknuckle)
# left and right inner knuckle
rtq85_innerknucklel.instanceTo(rtq85ilknuckle)
rtq85ilknuckle.setPos(-12.7, 61.42, 0)
rtq85ilknuckle.setHpr(0, 0, 180)
rtq85ilknuckle.reparentTo(rtq85base)
rtq85_innerknucklel.instanceTo(rtq85irknuckle)
rtq85irknuckle.setPos(12.7, 61.42, 0)
rtq85irknuckle.setHpr(0, 0, 0)
rtq85irknuckle.reparentTo(rtq85base)
# left and right fgr tip
rtq85_fingertipl.instanceTo(rtq85lfgrtip)
rtq85lfgrtip.setPos(37.59940821, 43.03959807, 0)
rtq85lfgrtip.reparentTo(rtq85ilknuckle)
rtq85_fingertipl.instanceTo(rtq85rfgrtip)
rtq85rfgrtip.setPos(37.59940821, 43.03959807, 0)
rtq85rfgrtip.setHpr(0, 0, 0)
rtq85rfgrtip.reparentTo(rtq85irknuckle)
# 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
rtq85base.setMat(pandageom.cvtMat4(rm.rodrigues([0,0,1], 90))*rtq85base.getMat())
rtq85base.reparentTo(self.rtq85np)
self.setJawwidth(jawwidth)
self.__jawwidthopen = 85.0
self.__jawwidthclosed = 0.0
def saveToDB(self, positionlist, gdb, discretesize=8.0):
"""
:param positionlist: a list of positions to place the object one the table
:param discretesize: the discretization of rotation angles around z axis
:return:
author: weiwei
date: 20161215, osaka
"""
# save discretiezed angle
sql = "SELECT * FROM angle"
result = gdb.execute(sql)
if len(result) == 0:
sql = "INSERT INTO angle(value) VALUES "
for i in range(discretesize):
sql += "("+str(360*i*1.0/discretesize)+"), "
sql = sql[:-2]+";"
gdb.execute(sql)
else:
print "Angles already set!"
# save tabletopplacements
sql = "SELECT idtabletopplacements FROM tabletopplacements,freetabletopplacement,object WHERE \
tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
# 1) select the freetabletopplacement
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
result = np.asarray(result)
idfreelist = [int(x) for x in result[:, 0]]
rotmatfreelist = [dc.strToMat4(x) for x in result[:, 1]]
# 2) select the angle
sql = "SELECT angle.idangle,angle.value FROM angle"
result = np.asarray(gdb.execute(sql))
idanglelist = [int(x) for x in result[:, 0]]
anglevaluelist = [float(x) for x in result[:, 1]]
# 3) save to database
sql = "INSERT INTO tabletopplacements(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES "
for ttoppos in positionlist:
ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2])
for idfree, rotmatfree in zip(idfreelist, rotmatfreelist):
for idangle, anglevalue in zip(idanglelist, anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2]+";"
gdb.execute(sql)
else:
print "Tabletopplacements already exist!"
# save tabletopgrips
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT tabletopgrips.idtabletopgrips FROM tabletopgrips,freeairgrip,object WHERE \
tabletopgrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \
freeairgrip.idobject=object.idobject AND object.name LIKE '%s' AND \
freeairgrip.idhand = %d" % (self.dbobjname, idhand)
result = gdb.execute(sql)
if len(result) == 0:
sql = "SELECT freetabletopplacement.idfreetabletopplacement \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
for idfree in result:
idfree = int(idfree[0])
sql = "SELECT tabletopplacements.idtabletopplacements, \
tabletopplacements.tabletopposition, angle.value,\
freetabletopgrip.contactpoint0, freetabletopgrip.contactpoint1, \
freetabletopgrip.contactnormal0, freetabletopgrip.contactnormal1, \
freetabletopgrip.rotmat, freetabletopgrip.jawwidth, freetabletopgrip.idfreeairgrip \
FROM tabletopplacements,freetabletopplacement,freetabletopgrip,freeairgrip,angle WHERE \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
tabletopplacements.idangle = angle.idangle AND \
freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = %d AND \
freetabletopplacement.idfreetabletopplacement = %d" % (idhand, idfree)
result1 = gdb.execute(sql)
if len(result1) == 0:
# no grasp availalbe?
continue
# sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, \
# contactnormal1, rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES "
if len(result1) > 20000:
result1 = result1[0::int(len(result1)/20000.0)]
result1 = np.asarray(result1)
idtabletopplacementslist = [int(x) for x in result1[:,0]]
tabletoppositionlist = [dc.strToV3(x) for x in result1[:,1]]
rotanglelist = [float(x) for x in result1[:,2]]
freegripcontactpoint0list = [dc.strToV3(x) for x in result1[:,3]]
freegripcontactpoint1list = [dc.strToV3(x) for x in result1[:,4]]
freegripcontactnormal0list = [dc.strToV3(x) for x in result1[:,5]]
freegripcontactnormal1list = [dc.strToV3(x) for x in result1[:,6]]
freegriprotmatlist = [dc.strToMat4(x) for x in result1[:,7]]
freegripjawwidthlist = [float(x) for x in result1[:,8]]
freegripidlist = [int(x) for x in result1[:,9]]
for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \
freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \
tabletoppositionlist, rotanglelist, freegripcontactpoint0list, freegripcontactpoint1list, \
freegripcontactnormal0list, freegripcontactnormal1list, freegriprotmatlist, freegripjawwidthlist, \
freegripidlist):
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
ttpcct0 = rotmat4.xformPoint(cct0)
ttpcct1 = rotmat4.xformPoint(cct1)
ttpcctn0 = rotmat4.xformVec(cctn0)
ttpcctn1 = rotmat4.xformVec(cctn1)
ttpgriprotmat = freegriprotmat*rotmat4
sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES \
('%s', '%s', '%s', '%s', '%s', '%s', %d, %d) " % \
(dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \
dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements)
gdb.execute(sql)
else:
print "Tabletopgrips already exist!"
print "Save to DB done!"
def genmnp(nxtrobot, handpkg, jawwidthrgt=None, jawwidthlft=None):
"""
generate a panda3d nodepath for the nxtrobot
mnp indicates this function generates a mesh nodepath
:param nxtrobot: the NxtRobot object, see nextage.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20161109
"""
nxtmnp = NodePath("nxtmnp")
this_dir, this_filename = os.path.split(__file__)
# mainbody, waist, body, head (neck is not plotted)
nxtwaist_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_waist.egg"))
nxtbody_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_body.egg"))
nxthead_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_head.egg"))
nxtwaist_model = loader.loadModel(nxtwaist_filepath)
nxtbody_model = loader.loadModel(nxtbody_filepath)
nxthead_model = loader.loadModel(nxthead_filepath)
nxtwaist_nodepath = NodePath("nxtwaist")
nxtbody_nodepath = NodePath("nxtbody")
nxthead_nodepath = NodePath("nxthead")
nxtwaist_model.instanceTo(nxtwaist_nodepath)
nxtwaist_rotmat = pandageom.cvtMat4(nxtrobot.base['rotmat'])
nxtwaist_nodepath.setMat(nxtwaist_rotmat)
nxtbody_model.instanceTo(nxtbody_nodepath)
nxthead_model.instanceTo(nxthead_nodepath)
nxthead_nodepath.setH(nxtrobot.initjnts[1])
nxthead_nodepath.setP(nxtrobot.initjnts[2])
nxthead_nodepath.setZ(569.5)
nxtwaist_nodepath.reparentTo(nxtmnp)
nxtbody_nodepath.reparentTo(nxtwaist_nodepath)
nxthead_nodepath.reparentTo(nxtwaist_nodepath)
# rgtarm
nxtrgtarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj0.egg"))
nxtrgtarmlj0_model = loader.loadModel(nxtrgtarmlj0_filepath)
nxtrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath")
nxtrgtarmlj0_model.instanceTo(nxtrgtarmlj0_nodepath)
nxtrgtarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[1]['rotmat'],
nxtrobot.rgtarm[1]['linkpos'])
nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat)
nxtrgtarmlj0_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj1.egg"))
nxtrgtarmlj1_model = loader.loadModel(nxtrgtarmlj1_filepath)
nxtrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath")
nxtrgtarmlj1_model.instanceTo(nxtrgtarmlj1_nodepath)
nxtrgtarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[2]['rotmat'],
nxtrobot.rgtarm[2]['linkpos'])
nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat)
nxtrgtarmlj1_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj2.egg"))
nxtrgtarmlj2_model = loader.loadModel(nxtrgtarmlj2_filepath)
nxtrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath")
nxtrgtarmlj2_model.instanceTo(nxtrgtarmlj2_nodepath)
nxtrgtarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[3]['rotmat'],
nxtrobot.rgtarm[3]['linkpos'])
nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat)
nxtrgtarmlj2_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj3.egg"))
nxtrgtarmlj3_model = loader.loadModel(nxtrgtarmlj3_filepath)
nxtrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath")
nxtrgtarmlj3_model.instanceTo(nxtrgtarmlj3_nodepath)
nxtrgtarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[4]['rotmat'],
nxtrobot.rgtarm[4]['linkpos'])
nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat)
nxtrgtarmlj3_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj4.egg"))
nxtrgtarmlj4_model = loader.loadModel(nxtrgtarmlj4_filepath)
nxtrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath")
nxtrgtarmlj4_model.instanceTo(nxtrgtarmlj4_nodepath)
nxtrgtarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[5]['rotmat'],
nxtrobot.rgtarm[5]['linkpos'])
nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat)
nxtrgtarmlj4_nodepath.reparentTo(nxtmnp)
# lftarm
nxtlftarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj0.egg"))
nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath)
nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath")
nxtlftarmlj0_model.instanceTo(nxtlftarmlj0_nodepath)
nxtlftarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[1]['rotmat'],
nxtrobot.lftarm[1]['linkpos'])
nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat)
nxtlftarmlj0_nodepath.reparentTo(nxtmnp)
nxtlftarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj1.egg"))
nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath)
nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath")
nxtlftarmlj1_model.instanceTo(nxtlftarmlj1_nodepath)
nxtlftarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[2]['rotmat'],
nxtrobot.lftarm[2]['linkpos'])
nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat)
nxtlftarmlj1_nodepath.reparentTo(nxtmnp)
nxtlftarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj2.egg"))
nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath)
nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath")
nxtlftarmlj2_model.instanceTo(nxtlftarmlj2_nodepath)
nxtlftarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[3]['rotmat'],
nxtrobot.lftarm[3]['linkpos'])
nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat)
nxtlftarmlj2_nodepath.reparentTo(nxtmnp)
nxtlftarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj3.egg"))
nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath)
nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath")
nxtlftarmlj3_model.instanceTo(nxtlftarmlj3_nodepath)
nxtlftarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[4]['rotmat'],
nxtrobot.lftarm[4]['linkpos'])
nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat)
nxtlftarmlj3_nodepath.reparentTo(nxtmnp)
nxtlftarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj4.egg"))
nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath)
nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath")
nxtlftarmlj4_model.instanceTo(nxtlftarmlj4_nodepath)
nxtlftarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[5]['rotmat'],
nxtrobot.lftarm[5]['linkpos'])
nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat)
nxtlftarmlj4_nodepath.reparentTo(nxtmnp)
# rgthnd
nxtrgthnd = handpkg.newHand('rgt')
nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'],
nxtrobot.rgtarm[6]['linkpos'])
nxtrgthnd.setMat(nxtlftarmlj5_rotmat)
nxtrgthnd.reparentTo(nxtmnp)
if jawwidthrgt is not None:
nxtrgthnd.setJawwidth(jawwidthrgt)
# lfthnd
nxtlfthnd = handpkg.newHand('lft')
nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[6]['rotmat'],
nxtrobot.lftarm[6]['linkpos'])
nxtlfthnd.setMat(nxtlftarmlj5_rotmat)
nxtlfthnd.reparentTo(nxtmnp)
if jawwidthlft is not None:
nxtlfthnd.setJawwidth(jawwidthlft)
return nxtmnp
def genmnp_nm(nxtrobot,
handpkg,
plotcolor=[.5, .5, .5, .3],
jawwidthrgt=None,
jawwidthlft=None):
"""
generate a panda3d nodepath for the nxtrobot
mnp indicates this function generates a mesh nodepath
nm means no material
:param nxtrobot: the NxtRobot object, see nextage.py
:param plotcolor: the color of the model, alpha allowed
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20161109
"""
nxtmnp = NodePath("nxtmnp")
this_dir, this_filename = os.path.split(__file__)
# mainbody, waist, body, head (neck is not plotted)
nxtwaist_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_waist_nm.egg"))
nxtbody_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_body_nm.egg"))
nxthead_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_head_nm.egg"))
nxtwaist_model = loader.loadModel(nxtwaist_filepath)
nxtbody_model = loader.loadModel(nxtbody_filepath)
nxthead_model = loader.loadModel(nxthead_filepath)
nxtwaist_nodepath = NodePath("nxtwaist_nm")
nxtbody_nodepath = NodePath("nxtbody_nm")
nxthead_nodepath = NodePath("nxthead_nm")
nxtwaist_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxtwaist_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtbody_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxtbody_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxthead_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxthead_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtwaist_model.instanceTo(nxtwaist_nodepath)
nxtwaist_rotmat = pandageom.cvtMat4(nxtrobot.base['rotmat'])
nxtwaist_nodepath.setMat(nxtwaist_rotmat)
nxtbody_model.instanceTo(nxtbody_nodepath)
nxthead_model.instanceTo(nxthead_nodepath)
nxthead_nodepath.setH(nxtrobot.initjnts[1])
nxthead_nodepath.setP(nxtrobot.initjnts[2])
nxthead_nodepath.setZ(569.5)
nxtwaist_nodepath.reparentTo(nxtmnp)
nxtbody_nodepath.reparentTo(nxtwaist_nodepath)
nxthead_nodepath.reparentTo(nxtwaist_nodepath)
# rgtarm
nxtrgtarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj0_nm.egg"))
nxtrgtarmlj0_model = loader.loadModel(nxtrgtarmlj0_filepath)
nxtrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath")
nxtrgtarmlj0_model.instanceTo(nxtrgtarmlj0_nodepath)
nxtrgtarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[1]['rotmat'],
nxtrobot.rgtarm[1]['linkpos'])
nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat)
nxtrgtarmlj0_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj0_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxtrgtarmlj0_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtrgtarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj1_nm.egg"))
nxtrgtarmlj1_model = loader.loadModel(nxtrgtarmlj1_filepath)
nxtrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath")
nxtrgtarmlj1_model.instanceTo(nxtrgtarmlj1_nodepath)
nxtrgtarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[2]['rotmat'],
nxtrobot.rgtarm[2]['linkpos'])
nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat)
nxtrgtarmlj1_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxtrgtarmlj1_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtrgtarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj2_nm.egg"))
nxtrgtarmlj2_model = loader.loadModel(nxtrgtarmlj2_filepath)
nxtrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath")
nxtrgtarmlj2_model.instanceTo(nxtrgtarmlj2_nodepath)
nxtrgtarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[3]['rotmat'],
nxtrobot.rgtarm[3]['linkpos'])
nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat)
nxtrgtarmlj2_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxtrgtarmlj2_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtrgtarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj3_nm.egg"))
nxtrgtarmlj3_model = loader.loadModel(nxtrgtarmlj3_filepath)
nxtrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath")
nxtrgtarmlj3_model.instanceTo(nxtrgtarmlj3_nodepath)
nxtrgtarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[4]['rotmat'],
nxtrobot.rgtarm[4]['linkpos'])
nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat)
nxtrgtarmlj3_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj3_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxtrgtarmlj3_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtrgtarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj4_nm.egg"))
nxtrgtarmlj4_model = loader.loadModel(nxtrgtarmlj4_filepath)
nxtrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath")
nxtrgtarmlj4_model.instanceTo(nxtrgtarmlj4_nodepath)
nxtrgtarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[5]['rotmat'],
nxtrobot.rgtarm[5]['linkpos'])
nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat)
nxtrgtarmlj4_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj4_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxtrgtarmlj4_nodepath.setTransparency(TransparencyAttrib.MAlpha)
# lftarm
nxtlftarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj0_nm.egg"))
nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath)
nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath")
nxtlftarmlj0_model.instanceTo(nxtlftarmlj0_nodepath)
nxtlftarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[1]['rotmat'],
nxtrobot.lftarm[1]['linkpos'])
nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat)
nxtlftarmlj0_nodepath.reparentTo(nxtmnp)
nxtlftarmlj0_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxtlftarmlj0_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtlftarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj1_nm.egg"))
nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath)
nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath")
nxtlftarmlj1_model.instanceTo(nxtlftarmlj1_nodepath)
nxtlftarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[2]['rotmat'],
nxtrobot.lftarm[2]['linkpos'])
nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat)
nxtlftarmlj1_nodepath.reparentTo(nxtmnp)
nxtlftarmlj1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxtlftarmlj1_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtlftarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj2_nm.egg"))
nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath)
nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath")
nxtlftarmlj2_model.instanceTo(nxtlftarmlj2_nodepath)
nxtlftarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[3]['rotmat'],
nxtrobot.lftarm[3]['linkpos'])
nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat)
nxtlftarmlj2_nodepath.reparentTo(nxtmnp)
nxtlftarmlj2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxtlftarmlj2_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtlftarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj3_nm.egg"))
nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath)
nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath")
nxtlftarmlj3_model.instanceTo(nxtlftarmlj3_nodepath)
nxtlftarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[4]['rotmat'],
nxtrobot.lftarm[4]['linkpos'])
nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat)
nxtlftarmlj3_nodepath.reparentTo(nxtmnp)
nxtlftarmlj3_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxtlftarmlj3_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtlftarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj4_nm.egg"))
nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath)
nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath")
nxtlftarmlj4_model.instanceTo(nxtlftarmlj4_nodepath)
nxtlftarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[5]['rotmat'],
nxtrobot.lftarm[5]['linkpos'])
nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat)
nxtlftarmlj4_nodepath.reparentTo(nxtmnp)
nxtlftarmlj4_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2],
plotcolor[3])
nxtlftarmlj4_nodepath.setTransparency(TransparencyAttrib.MAlpha)
# rgthnd
nxtrgthnd = handpkg.newHandNM('rgt', hndcolor=plotcolor)
nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'],
nxtrobot.rgtarm[6]['linkpos'])
nxtrgthnd.setMat(nxtlftarmlj5_rotmat)
nxtrgthnd.reparentTo(nxtmnp)
if jawwidthrgt is not None:
nxtrgthnd.setJawwidth(jawwidthrgt)
# lfthnd
nxtlfthnd = handpkg.newHandNM('lft', hndcolor=plotcolor)
nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[6]['rotmat'],
nxtrobot.lftarm[6]['linkpos'])
nxtlfthnd.setMat(nxtlftarmlj5_rotmat)
nxtlfthnd.reparentTo(nxtmnp)
if jawwidthlft is not None:
nxtlfthnd.setJawwidth(jawwidthlft)
nxtmnp.setTransparency(TransparencyAttrib.MAlpha)
return nxtmnp
import hrp5nplot
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]])
# plot goal axis
pg.plotAxisSelf(nodepath=base.render,
spos=objpos,
pandamat4=pg.cvtMat4(objrot))
# 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)
def genmnp_nm(hrp5nrobot, handpkg, jawwidthrgt = None, jawwidthlft = None):
"""
generate a panda3d nodepath for the hrp5robot
mnp indicates this function generates a mesh nodepath
:param hrp5robot: the Hrp5Robot object, see hrp5.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20161202
"""
hrp5nmnp = NodePath("hrp5nmnp")
this_dir, this_filename = os.path.split(__file__)
# chest0-2, head1 (neck is not plotted)
hrp5nbody_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg/nomat", "HRP-5P_ConceptDesign_Body.egg"))
hrp5nchest0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg/nomat", "HRP-5P_ConceptDesign_Chest_Link0.egg"))
hrp5nchest1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg/nomat", "HRP-5P_ConceptDesign_Chest_Link1.egg"))
hrp5nchest2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg/nomat", "HRP-5P_ConceptDesign_Chest_Link2.egg"))
hrp5nhead0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg/nomat", "HRP-5P_ConceptDesign_Head_Link0.egg"))
hrp5nhead1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg/nomat", "HRP-5P_ConceptDesign_Head_Link1.egg"))
hrp5nbody_model = loader.loadModel(hrp5nbody_filepath)
hrp5nchest0_model = loader.loadModel(hrp5nchest0_filepath)
hrp5nchest1_model = loader.loadModel(hrp5nchest1_filepath)
hrp5nchest2_model = loader.loadModel(hrp5nchest2_filepath)
hrp5nhead0_model = loader.loadModel(hrp5nhead0_filepath)
hrp5nhead1_model = loader.loadModel(hrp5nhead1_filepath)
hrp5nbody_nodepath = NodePath("hrp5nbody")
hrp5nchest0_nodepath = NodePath("hrp5nchest0")
hrp5nchest1_nodepath = NodePath("hrp5nchest1")
hrp5nchest2_nodepath = NodePath("hrp5nchest2")
hrp5nhead0_nodepath = NodePath("hrp5nhead0")
hrp5nhead1_nodepath = NodePath("hrp5nhead1")
# body
hrp5nbody_model.instanceTo(hrp5nbody_nodepath)
hrp5nbody_rotmat = Mat4.identMat()
hrp5nbody_nodepath.setMat(hrp5nbody_rotmat)
hrp5nbody_nodepath.setZ(0)
# chest
hrp5nchest0_model.instanceTo(hrp5nchest0_nodepath)
hrp5nchest0_rotmat = Mat4.identMat()
hrp5nchest0_nodepath.setMat(hrp5nchest0_rotmat)
hrp5nchest0_nodepath.setZ(274)
hrp5nchest1_model.instanceTo(hrp5nchest1_nodepath)
hrp5nchest1_nodepath.setColor(.7,.7,.2,1)
hrp5nchest2_model.instanceTo(hrp5nchest2_nodepath)
hrp5nchest2_rotmat = pg.cvtMat4(hrp5nrobot.base['rotmat'])
hrp5nchest2_nodepath.setMat(hrp5nchest2_rotmat)
hrp5nchest2_nodepath.setColor(.7,.7,.7,1)
hrp5nhead0_model.instanceTo(hrp5nhead0_nodepath)
hrp5nhead0_nodepath.setH(hrp5nrobot.initjnts[0])
hrp5nhead0_nodepath.setX(32)
hrp5nhead0_nodepath.setZ(521)
hrp5nhead1_model.instanceTo(hrp5nhead1_nodepath)
hrp5nhead1_nodepath.setP(hrp5nrobot.initjnts[1])
hrp5nhead1_nodepath.setX(40)
hrp5nhead1_nodepath.setColor(.5,.5,.5,1)
hrp5nbody_nodepath.reparentTo(hrp5nmnp)
hrp5nchest0_nodepath.reparentTo(hrp5nbody_nodepath)
hrp5nchest1_nodepath.reparentTo(hrp5nchest0_nodepath)
hrp5nchest2_nodepath.reparentTo(hrp5nchest1_nodepath)
hrp5nhead0_nodepath.reparentTo(hrp5nchest2_nodepath)
hrp5nhead1_nodepath.reparentTo(hrp5nhead0_nodepath)
# rgtarm
hrp5nrgtarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rscap_Link0.egg"))
hrp5nrgtarmlj0_model = loader.loadModel(hrp5nrgtarmlj0_filepath)
hrp5nrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath")
hrp5nrgtarmlj0_model.instanceTo(hrp5nrgtarmlj0_nodepath)
hrp5nrgtarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[1]['rotmat'], hrp5nrobot.rgtarm[1]['linkpos'])
hrp5nrgtarmlj0_nodepath.setMat(hrp5nrgtarmlj0_rotmat)
hrp5nrgtarmlj0_nodepath.setColor(.5,.5,.5,1)
hrp5nrgtarmlj0_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link0.egg"))
hrp5nrgtarmlj1_model = loader.loadModel(hrp5nrgtarmlj1_filepath)
hrp5nrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath")
hrp5nrgtarmlj1_model.instanceTo(hrp5nrgtarmlj1_nodepath)
hrp5nrgtarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[2]['rotmat'], hrp5nrobot.rgtarm[2]['linkpos'])
hrp5nrgtarmlj1_nodepath.setMat(hrp5nrgtarmlj1_rotmat)
hrp5nrgtarmlj1_nodepath.setColor(.5,.5,.5,1)
hrp5nrgtarmlj1_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link1.egg"))
hrp5nrgtarmlj2_model = loader.loadModel(hrp5nrgtarmlj2_filepath)
hrp5nrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath")
hrp5nrgtarmlj2_model.instanceTo(hrp5nrgtarmlj2_nodepath)
hrp5nrgtarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[3]['rotmat'], hrp5nrobot.rgtarm[3]['linkpos'])
hrp5nrgtarmlj2_nodepath.setMat(hrp5nrgtarmlj2_rotmat)
hrp5nrgtarmlj2_nodepath.setColor(.5,.5,.5,1)
hrp5nrgtarmlj2_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link2.egg"))
hrp5nrgtarmlj3_model = loader.loadModel(hrp5nrgtarmlj3_filepath)
hrp5nrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath")
hrp5nrgtarmlj3_model.instanceTo(hrp5nrgtarmlj3_nodepath)
hrp5nrgtarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[4]['rotmat'], hrp5nrobot.rgtarm[4]['linkpos'])
hrp5nrgtarmlj3_nodepath.setMat(hrp5nrgtarmlj3_rotmat)
hrp5nrgtarmlj3_nodepath.setColor(.5,.5,.5,1)
hrp5nrgtarmlj3_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link3.egg"))
hrp5nrgtarmlj4_model = loader.loadModel(hrp5nrgtarmlj4_filepath)
hrp5nrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath")
hrp5nrgtarmlj4_model.instanceTo(hrp5nrgtarmlj4_nodepath)
hrp5nrgtarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[5]['rotmat'], hrp5nrobot.rgtarm[5]['linkpos'])
hrp5nrgtarmlj4_nodepath.setMat(hrp5nrgtarmlj4_rotmat)
hrp5nrgtarmlj4_nodepath.setColor(.5,.5,.5,1)
hrp5nrgtarmlj4_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj5_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link4.egg"))
hrp5nrgtarmlj5_model = loader.loadModel(hrp5nrgtarmlj5_filepath)
hrp5nrgtarmlj5_nodepath = NodePath("nxtrgtarmlj5_nodepath")
hrp5nrgtarmlj5_model.instanceTo(hrp5nrgtarmlj5_nodepath)
hrp5nrgtarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[6]['rotmat'], hrp5nrobot.rgtarm[6]['linkpos'])
hrp5nrgtarmlj5_nodepath.setMat(hrp5nrgtarmlj5_rotmat)
hrp5nrgtarmlj5_nodepath.setColor(.5,.5,.5,1)
hrp5nrgtarmlj5_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj6_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link5.egg"))
hrp5nrgtarmlj6_model = loader.loadModel(hrp5nrgtarmlj6_filepath)
hrp5nrgtarmlj6_nodepath = NodePath("nxtrgtarmlj6_nodepath")
hrp5nrgtarmlj6_model.instanceTo(hrp5nrgtarmlj6_nodepath)
hrp5nrgtarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[7]['rotmat'], hrp5nrobot.rgtarm[7]['linkpos'])
hrp5nrgtarmlj6_nodepath.setMat(hrp5nrgtarmlj6_rotmat)
hrp5nrgtarmlj6_nodepath.setColor(.5,.5,.5,1)
hrp5nrgtarmlj6_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj7_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link6.egg"))
hrp5nrgtarmlj7_model = loader.loadModel(hrp5nrgtarmlj7_filepath)
hrp5nrgtarmlj7_nodepath = NodePath("nxtrgtarmlj7_nodepath")
hrp5nrgtarmlj7_model.instanceTo(hrp5nrgtarmlj7_nodepath)
hrp5nrgtarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[8]['rotmat'], hrp5nrobot.rgtarm[8]['linkpos'])
hrp5nrgtarmlj7_nodepath.setMat(hrp5nrgtarmlj7_rotmat)
hrp5nrgtarmlj7_nodepath.setColor(.5,.5,.5,1)
hrp5nrgtarmlj7_nodepath.reparentTo(hrp5nmnp)
#
# rgtarm
hrp5nrgtarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rscap_Link0.egg"))
hrp5nrgtarmlj0_model = loader.loadModel(hrp5nrgtarmlj0_filepath)
hrp5nrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath")
hrp5nrgtarmlj0_model.instanceTo(hrp5nrgtarmlj0_nodepath)
hrp5nrgtarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[1]['rotmat'], hrp5nrobot.rgtarm[1]['linkpos'])
hrp5nrgtarmlj0_nodepath.setMat(hrp5nrgtarmlj0_rotmat)
hrp5nrgtarmlj0_nodepath.setColor(.5, .5, .5, 1)
hrp5nrgtarmlj0_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link0.egg"))
hrp5nrgtarmlj1_model = loader.loadModel(hrp5nrgtarmlj1_filepath)
hrp5nrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath")
hrp5nrgtarmlj1_model.instanceTo(hrp5nrgtarmlj1_nodepath)
hrp5nrgtarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[2]['rotmat'], hrp5nrobot.rgtarm[2]['linkpos'])
hrp5nrgtarmlj1_nodepath.setMat(hrp5nrgtarmlj1_rotmat)
hrp5nrgtarmlj1_nodepath.setColor(.5, .5, .5, 1)
hrp5nrgtarmlj1_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link1.egg"))
hrp5nrgtarmlj2_model = loader.loadModel(hrp5nrgtarmlj2_filepath)
hrp5nrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath")
hrp5nrgtarmlj2_model.instanceTo(hrp5nrgtarmlj2_nodepath)
hrp5nrgtarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[3]['rotmat'], hrp5nrobot.rgtarm[3]['linkpos'])
hrp5nrgtarmlj2_nodepath.setMat(hrp5nrgtarmlj2_rotmat)
hrp5nrgtarmlj2_nodepath.setColor(.5, .5, .5, 1)
hrp5nrgtarmlj2_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link2.egg"))
hrp5nrgtarmlj3_model = loader.loadModel(hrp5nrgtarmlj3_filepath)
hrp5nrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath")
hrp5nrgtarmlj3_model.instanceTo(hrp5nrgtarmlj3_nodepath)
hrp5nrgtarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[4]['rotmat'], hrp5nrobot.rgtarm[4]['linkpos'])
hrp5nrgtarmlj3_nodepath.setMat(hrp5nrgtarmlj3_rotmat)
hrp5nrgtarmlj3_nodepath.setColor(.5, .5, .5, 1)
hrp5nrgtarmlj3_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link3.egg"))
hrp5nrgtarmlj4_model = loader.loadModel(hrp5nrgtarmlj4_filepath)
hrp5nrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath")
hrp5nrgtarmlj4_model.instanceTo(hrp5nrgtarmlj4_nodepath)
hrp5nrgtarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[5]['rotmat'], hrp5nrobot.rgtarm[5]['linkpos'])
hrp5nrgtarmlj4_nodepath.setMat(hrp5nrgtarmlj4_rotmat)
hrp5nrgtarmlj4_nodepath.setColor(.5, .5, .5, 1)
hrp5nrgtarmlj4_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj5_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link4.egg"))
hrp5nrgtarmlj5_model = loader.loadModel(hrp5nrgtarmlj5_filepath)
hrp5nrgtarmlj5_nodepath = NodePath("nxtrgtarmlj5_nodepath")
hrp5nrgtarmlj5_model.instanceTo(hrp5nrgtarmlj5_nodepath)
hrp5nrgtarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[6]['rotmat'], hrp5nrobot.rgtarm[6]['linkpos'])
hrp5nrgtarmlj5_nodepath.setMat(hrp5nrgtarmlj5_rotmat)
hrp5nrgtarmlj5_nodepath.setColor(.5, .5, .5, 1)
hrp5nrgtarmlj5_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj6_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link5.egg"))
hrp5nrgtarmlj6_model = loader.loadModel(hrp5nrgtarmlj6_filepath)
hrp5nrgtarmlj6_nodepath = NodePath("nxtrgtarmlj6_nodepath")
hrp5nrgtarmlj6_model.instanceTo(hrp5nrgtarmlj6_nodepath)
hrp5nrgtarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[7]['rotmat'], hrp5nrobot.rgtarm[7]['linkpos'])
hrp5nrgtarmlj6_nodepath.setMat(hrp5nrgtarmlj6_rotmat)
hrp5nrgtarmlj6_nodepath.setColor(.5, .5, .5, 1)
hrp5nrgtarmlj6_nodepath.reparentTo(hrp5nmnp)
#
hrp5nrgtarmlj7_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link6.egg"))
hrp5nrgtarmlj7_model = loader.loadModel(hrp5nrgtarmlj7_filepath)
hrp5nrgtarmlj7_nodepath = NodePath("nxtrgtarmlj7_nodepath")
hrp5nrgtarmlj7_model.instanceTo(hrp5nrgtarmlj7_nodepath)
hrp5nrgtarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[8]['rotmat'], hrp5nrobot.rgtarm[8]['linkpos'])
hrp5nrgtarmlj7_nodepath.setMat(hrp5nrgtarmlj7_rotmat)
hrp5nrgtarmlj7_nodepath.setColor(.5, .5, .5, 1)
hrp5nrgtarmlj7_nodepath.reparentTo(hrp5nmnp)
# lftarm
hrp5nlftarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Lscap_Link0.egg"))
hrp5nlftarmlj0_model = loader.loadModel(hrp5nlftarmlj0_filepath)
hrp5nlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath")
hrp5nlftarmlj0_model.instanceTo(hrp5nlftarmlj0_nodepath)
hrp5nlftarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[1]['rotmat'], hrp5nrobot.lftarm[1]['linkpos'])
hrp5nlftarmlj0_nodepath.setMat(hrp5nlftarmlj0_rotmat)
hrp5nlftarmlj0_nodepath.setColor(.5, .5, .5, 1)
hrp5nlftarmlj0_nodepath.reparentTo(hrp5nmnp)
#
hrp5nlftarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link0.egg"))
hrp5nlftarmlj1_model = loader.loadModel(hrp5nlftarmlj1_filepath)
hrp5nlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath")
hrp5nlftarmlj1_model.instanceTo(hrp5nlftarmlj1_nodepath)
hrp5nlftarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[2]['rotmat'], hrp5nrobot.lftarm[2]['linkpos'])
hrp5nlftarmlj1_nodepath.setMat(hrp5nlftarmlj1_rotmat)
hrp5nlftarmlj1_nodepath.setColor(.5, .5, .5, 1)
hrp5nlftarmlj1_nodepath.reparentTo(hrp5nmnp)
#
hrp5nlftarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link1.egg"))
hrp5nlftarmlj2_model = loader.loadModel(hrp5nlftarmlj2_filepath)
hrp5nlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath")
hrp5nlftarmlj2_model.instanceTo(hrp5nlftarmlj2_nodepath)
hrp5nlftarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[3]['rotmat'], hrp5nrobot.lftarm[3]['linkpos'])
hrp5nlftarmlj2_nodepath.setMat(hrp5nlftarmlj2_rotmat)
hrp5nlftarmlj2_nodepath.setColor(.5, .5, .5, 1)
hrp5nlftarmlj2_nodepath.reparentTo(hrp5nmnp)
#
hrp5nlftarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link2.egg"))
hrp5nlftarmlj3_model = loader.loadModel(hrp5nlftarmlj3_filepath)
hrp5nlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath")
hrp5nlftarmlj3_model.instanceTo(hrp5nlftarmlj3_nodepath)
hrp5nlftarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[4]['rotmat'], hrp5nrobot.lftarm[4]['linkpos'])
hrp5nlftarmlj3_nodepath.setMat(hrp5nlftarmlj3_rotmat)
hrp5nlftarmlj3_nodepath.setColor(.5, .5, .5, 1)
hrp5nlftarmlj3_nodepath.reparentTo(hrp5nmnp)
#
hrp5nlftarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link3.egg"))
hrp5nlftarmlj4_model = loader.loadModel(hrp5nlftarmlj4_filepath)
hrp5nlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath")
hrp5nlftarmlj4_model.instanceTo(hrp5nlftarmlj4_nodepath)
hrp5nlftarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[5]['rotmat'], hrp5nrobot.lftarm[5]['linkpos'])
hrp5nlftarmlj4_nodepath.setMat(hrp5nlftarmlj4_rotmat)
hrp5nlftarmlj4_nodepath.setColor(.5, .5, .5, 1)
hrp5nlftarmlj4_nodepath.reparentTo(hrp5nmnp)
#
hrp5nlftarmlj5_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link4.egg"))
hrp5nlftarmlj5_model = loader.loadModel(hrp5nlftarmlj5_filepath)
hrp5nlftarmlj5_nodepath = NodePath("nxtlftarmlj5_nodepath")
hrp5nlftarmlj5_model.instanceTo(hrp5nlftarmlj5_nodepath)
hrp5nlftarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[6]['rotmat'], hrp5nrobot.lftarm[6]['linkpos'])
hrp5nlftarmlj5_nodepath.setMat(hrp5nlftarmlj5_rotmat)
hrp5nlftarmlj5_nodepath.setColor(.5, .5, .5, 1)
hrp5nlftarmlj5_nodepath.reparentTo(hrp5nmnp)
#
hrp5nlftarmlj6_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link5.egg"))
hrp5nlftarmlj6_model = loader.loadModel(hrp5nlftarmlj6_filepath)
hrp5nlftarmlj6_nodepath = NodePath("nxtlftarmlj6_nodepath")
hrp5nlftarmlj6_model.instanceTo(hrp5nlftarmlj6_nodepath)
hrp5nlftarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[7]['rotmat'], hrp5nrobot.lftarm[7]['linkpos'])
hrp5nlftarmlj6_nodepath.setMat(hrp5nlftarmlj6_rotmat)
hrp5nlftarmlj6_nodepath.setColor(.5, .5, .5, 1)
hrp5nlftarmlj6_nodepath.reparentTo(hrp5nmnp)
#
hrp5nlftarmlj7_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link6.egg"))
hrp5nlftarmlj7_model = loader.loadModel(hrp5nlftarmlj7_filepath)
hrp5nlftarmlj7_nodepath = NodePath("nxtlftarmlj7_nodepath")
hrp5nlftarmlj7_model.instanceTo(hrp5nlftarmlj7_nodepath)
hrp5nlftarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[8]['rotmat'], hrp5nrobot.lftarm[8]['linkpos'])
hrp5nlftarmlj7_nodepath.setMat(hrp5nlftarmlj7_rotmat)
hrp5nlftarmlj7_nodepath.setColor(.5, .5, .5, 1)
hrp5nlftarmlj7_nodepath.reparentTo(hrp5nmnp)
# rgthnd
hrp5nrobotrgthnd = handpkg.newHand('rgt')
hrp5nrobotrgtarmlj9_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[9]['rotmat'], hrp5nrobot.rgtarm[9]['linkpos'])
# pg.plotAxisSelf(hrp5nmnp, hrp5nrobot.rgtarm[9]['linkend'], hrp5nrobotrgtarmlj9_rotmat)
hrp5nrobotrgthnd.setMat(pandanpmat4 = hrp5nrobotrgtarmlj9_rotmat)
hrp5nrobotrgthnd.reparentTo(hrp5nmnp)
if jawwidthrgt is not None:
hrp5nrobotrgthnd.setJawwidth(jawwidthrgt)
# lfthnd
hrp5nrobotlfthnd = handpkg.newHand('lft')
hrp5nrobotlftarmlj9_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[9]['rotmat'], hrp5nrobot.lftarm[9]['linkpos'])
# pg.plotAxisSelf(hrp5nmnp, hrp5nrobot.lftarm[9]['linkend'], hrp5nrobotlftarmlj9_rotmat)
hrp5nrobotlfthnd.setMat(pandanpmat4 = hrp5nrobotlftarmlj9_rotmat)
hrp5nrobotlfthnd.reparentTo(hrp5nmnp)
if jawwidthlft is not None:
hrp5nrobotlfthnd.setJawwidth(jawwidthlft)
return hrp5nmnp
rbd = Rigidbody(mass=10.0,
pos=np.array([0, 0, 0.0]),
com=[0.0, 0.0, 0.0],
rotmat=rm.rodrigues(np.array([1, 0, 0]), 1.8),
inertiatensor=np.array([[1732.0, 0.0, 0.0],
[0.0, 1732.0, 0.0],
[0.0, 0.0, 3393.0]]))
rbd.linearv = np.array([0, 0, 0])
rbd.angularw = np.array([0, 0, 50])
this_dir, this_filename = os.path.split(__file__)
model_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "models", "top.egg"))
model = loader.loadModel(model_filepath)
model.reparentTo(base.render)
model.setMat(pg.cvtMat4(rbd.rotmat))
model.setPos(pg.npToV3(rbd.pos))
# base = pc.World(camp=[3000, 0, 3000], lookatp=[0, 0, 0])
#
# rbd = Rigidbody(mass = 1.0, pos=np.array([0, 0, 0.0]), rotmat=np.identity(3),
# inertiatensor=np.array([[80833.3, 0.0, 0.0], [0.0, 68333.3, 0.0], [0.0, 0.0, 14166.7]]))
# rbd.linearv = np.array([0,0,0])
# rbd.angularw = np.array([1, 1, 1])
#
# this_dir, this_filename = os.path.split(__file__)
# model_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "models", "box.egg"))
# model = loader.loadModel(model_filepath)
# model.reparentTo(base.render)
import csv
def genmnp_nm(self,
hrp5nrobot,
jawwidthrgt=None,
jawwidthlft=None,
plotcolor=[.5, .5, .5, .3]):
"""
generate a panda3d nodepath for the hrp5robot
mnp indicates this function generates a mesh nodepath
nm indicates the robots include no material
since I didn't remove the materials on the hrp5 model,
this function is essentially the same as genmnp_nm
:param hrp5robot: the Hrp5Robot object, see hrp5.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20170613
"""
identmat4 = Mat4.identMat()
# body
self.__hrp5nbody_nodepath.setMat(identmat4)
self.__hrp5nbody_nodepath.setZ(self.__hrp5nmnp, 0)
# chest 0
self.__hrp5nchest0_nodepath.setMat(identmat4)
self.__hrp5nchest0_nodepath.setZ(self.__hrp5nmnp, 274)
# chest 1
self.__hrp5nchest1_nodepath.setMat(identmat4)
self.__hrp5nchest1_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# chest 2
hrp5nchest2_rotmat = pg.cvtMat4(hrp5nrobot.base['rotmat'])
self.__hrp5nchest2_nodepath.setMat(hrp5nchest2_rotmat)
self.__hrp5nchest2_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# head 0
self.__hrp5nhead0_nodepath.setMat(identmat4)
self.__hrp5nhead0_nodepath.setH(hrp5nrobot.initjnts[0])
self.__hrp5nhead0_nodepath.setX(32)
self.__hrp5nhead0_nodepath.setZ(521)
# head 1
self.__hrp5nhead1_nodepath.setP(hrp5nrobot.initjnts[1])
self.__hrp5nhead1_nodepath.setX(40)
self.__hrp5nhead1_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# rgtarm 0
hrp5nrgtarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[1]['rotmat'],
hrp5nrobot.rgtarm[1]['linkpos'])
self.__hrp5nrgtarmlj0_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj0_rotmat)
self.__hrp5nrgtarmlj0_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# rgtarm 1
hrp5nrgtarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[2]['rotmat'],
hrp5nrobot.rgtarm[2]['linkpos'])
self.__hrp5nrgtarmlj1_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj1_rotmat)
self.__hrp5nrgtarmlj1_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# rgtarm 2
hrp5nrgtarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[3]['rotmat'],
hrp5nrobot.rgtarm[3]['linkpos'])
self.__hrp5nrgtarmlj2_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj2_rotmat)
self.__hrp5nrgtarmlj2_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# rgtarm 3
hrp5nrgtarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[4]['rotmat'],
hrp5nrobot.rgtarm[4]['linkpos'])
self.__hrp5nrgtarmlj3_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj3_rotmat)
self.__hrp5nrgtarmlj3_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# rgtarm 4
hrp5nrgtarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[5]['rotmat'],
hrp5nrobot.rgtarm[5]['linkpos'])
self.__hrp5nrgtarmlj4_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj4_rotmat)
self.__hrp5nrgtarmlj4_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# rgtarm 5
hrp5nrgtarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[6]['rotmat'],
hrp5nrobot.rgtarm[6]['linkpos'])
self.__hrp5nrgtarmlj5_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj5_rotmat)
self.__hrp5nrgtarmlj5_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# rgtarm 6
hrp5nrgtarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[7]['rotmat'],
hrp5nrobot.rgtarm[7]['linkpos'])
self.__hrp5nrgtarmlj6_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj6_rotmat)
self.__hrp5nrgtarmlj6_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# rgtarm 7
hrp5nrgtarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[8]['rotmat'],
hrp5nrobot.rgtarm[8]['linkpos'])
self.__hrp5nrgtarmlj7_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj7_rotmat)
self.__hrp5nrgtarmlj7_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# lftarm 0
hrp5nlftarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[1]['rotmat'],
hrp5nrobot.lftarm[1]['linkpos'])
self.__hrp5nlftarmlj0_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj0_rotmat)
self.__hrp5nlftarmlj0_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# lftarm 1
hrp5nlftarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[2]['rotmat'],
hrp5nrobot.lftarm[2]['linkpos'])
self.__hrp5nlftarmlj1_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj1_rotmat)
self.__hrp5nlftarmlj1_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# lftarm 2
hrp5nlftarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[3]['rotmat'],
hrp5nrobot.lftarm[3]['linkpos'])
self.__hrp5nlftarmlj2_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj2_rotmat)
self.__hrp5nlftarmlj2_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# lftarm 3
hrp5nlftarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[4]['rotmat'],
hrp5nrobot.lftarm[4]['linkpos'])
self.__hrp5nlftarmlj3_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj3_rotmat)
self.__hrp5nlftarmlj3_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# lftarm 4
hrp5nlftarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[5]['rotmat'],
hrp5nrobot.lftarm[5]['linkpos'])
self.__hrp5nlftarmlj4_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj4_rotmat)
self.__hrp5nlftarmlj4_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# lftarm 5
hrp5nlftarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[6]['rotmat'],
hrp5nrobot.lftarm[6]['linkpos'])
self.__hrp5nlftarmlj5_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj5_rotmat)
self.__hrp5nlftarmlj5_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# lftarm 6
hrp5nlftarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[7]['rotmat'],
hrp5nrobot.lftarm[7]['linkpos'])
self.__hrp5nlftarmlj6_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj6_rotmat)
self.__hrp5nlftarmlj6_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# lftarm 7
hrp5nlftarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[8]['rotmat'],
hrp5nrobot.lftarm[8]['linkpos'])
self.__hrp5nlftarmlj7_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj7_rotmat)
self.__hrp5nlftarmlj7_nodepath.setColor(plotcolor[0], plotcolor[1],
plotcolor[2], plotcolor[3])
# rgthand
hrp5nrobotrgtarmlj9_rotmat = pg.cvtMat4(
hrp5nrobot.rgtarm[9]['rotmat'], hrp5nrobot.rgtarm[9]['linkpos'])
self.hrp5nrobotrgthnd.setMat(self.__hrp5nmnp,
hrp5nrobotrgtarmlj9_rotmat)
if jawwidthrgt is not None:
self.hrp5nrobotrgthnd.setJawwidth(jawwidthrgt)
# lfthand
hrp5nrobotlftarmlj9_rotmat = pg.cvtMat4(
hrp5nrobot.lftarm[9]['rotmat'], hrp5nrobot.lftarm[9]['linkpos'])
self.hrp5nrobotlfthnd.setMat(self.__hrp5nmnp,
hrp5nrobotlftarmlj9_rotmat)
if jawwidthlft is not None:
self.hrp5nrobotlfthnd.setJawwidth(jawwidthlft)
return copy.deepcopy(self.__hrp5nmnp)
def genmnplist(ur5dual, handpkg, jawwidthrgt=None, jawwidthlft=None):
"""
generate a list of panda3d nodepath for the ur5dual
mnp indicates this function generates a mesh nodepath
the return value is in the following format:
[[rightarm mnp list], [leftarm mnp list], [body]]
# Right goes first!
# The order of an arm mnp list is from base to end-effector
:param ur5dual:
:return: a list of mesh nodepaths
author: weiwei
date: 20170608
"""
ur5mnp = NodePath("ur5dualmnp")
this_dir, this_filename = os.path.split(__file__)
# chest0-2, head1 (neck is not plotted)
ur5base_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "ur5egg", "base.egg"))
ur5upperarm_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "ur5egg", "upperarm.egg"))
ur5shoulder_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "ur5egg", "shoulder.egg"))
ur5forearm_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "ur5egg", "forearm.egg"))
ur5wrist1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "ur5egg", "wrist1.egg"))
ur5wrist2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "ur5egg", "wrist2.egg"))
ur5wrist3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "ur5egg", "wrist3.egg"))
ur5base_model = loader.loadModel(ur5base_filepath)
ur5upperarm_model = loader.loadModel(ur5upperarm_filepath)
ur5shoulder_model = loader.loadModel(ur5shoulder_filepath)
ur5forearm_model = loader.loadModel(ur5forearm_filepath)
ur5wrist1_model = loader.loadModel(ur5wrist1_filepath)
ur5wrist2_model = loader.loadModel(ur5wrist2_filepath)
ur5wrist3_model = loader.loadModel(ur5wrist3_filepath)
# rgt
ur5rgtbase_nodepath = NodePath("ur5rgtbase")
ur5rgtshoulder_nodepath = NodePath("ur5rgtshoulder")
ur5rgtupperarm_nodepath = NodePath("ur5rgtupperarm")
ur5rgtforearm_nodepath = NodePath("ur5rgtforearm")
ur5rgtwrist1_nodepath = NodePath("ur5rgtwrist1")
ur5rgtwrist2_nodepath = NodePath("ur5rgtwrist2")
ur5rgtwrist3_nodepath = NodePath("ur5rgtwrist3")
#
# base egg coordinates is retracted 89.2 along z to fit parameters
ur5base_model.instanceTo(ur5rgtbase_nodepath)
ur5base_rotmat = pg.cvtMat4(ur5dual.rgtarm[1]['inherentR'],
ur5dual.rgtarm[1]['linkpos'])
ur5rgtbase_nodepath.setMat(ur5base_rotmat)
ur5rgtbase_nodepath.setColor(.5, .5, .5, 1)
ur5rgtbase_nodepath.reparentTo(ur5mnp)
#
ur5shoulder_model.instanceTo(ur5rgtshoulder_nodepath)
ur5shoulder_rotmat = pg.cvtMat4(ur5dual.rgtarm[1]['rotmat'],
ur5dual.rgtarm[1]['linkpos'])
ur5rgtshoulder_nodepath.setMat(ur5shoulder_rotmat)
ur5rgtshoulder_nodepath.setColor(.5, .7, .3, 1)
ur5rgtshoulder_nodepath.reparentTo(ur5mnp)
#
ur5upperarm_model.instanceTo(ur5rgtupperarm_nodepath)
ur5upperarm_rotmat = pg.cvtMat4(ur5dual.rgtarm[2]['rotmat'],
ur5dual.rgtarm[2]['linkpos'])
ur5rgtupperarm_nodepath.setMat(ur5upperarm_rotmat)
ur5rgtupperarm_nodepath.setColor(.5, .5, .5, 1)
ur5rgtupperarm_nodepath.reparentTo(ur5mnp)
#
ur5forearm_model.instanceTo(ur5rgtforearm_nodepath)
ur5forearm_rotmat = pg.cvtMat4(ur5dual.rgtarm[3]['rotmat'],
ur5dual.rgtarm[3]['linkpos'])
ur5rgtforearm_nodepath.setMat(ur5forearm_rotmat)
ur5rgtforearm_nodepath.setColor(.5, .5, .5, 1)
ur5rgtforearm_nodepath.reparentTo(ur5mnp)
#
ur5wrist1_model.instanceTo(ur5rgtwrist1_nodepath)
ur5wrist1_rotmat = pg.cvtMat4(ur5dual.rgtarm[4]['rotmat'],
ur5dual.rgtarm[4]['linkpos'])
ur5rgtwrist1_nodepath.setMat(ur5wrist1_rotmat)
ur5rgtwrist1_nodepath.setColor(.5, .7, .3, 1)
ur5rgtwrist1_nodepath.reparentTo(ur5mnp)
#
ur5wrist2_model.instanceTo(ur5rgtwrist2_nodepath)
ur5wrist2_rotmat = pg.cvtMat4(ur5dual.rgtarm[5]['rotmat'],
ur5dual.rgtarm[5]['linkpos'])
ur5rgtwrist2_nodepath.setMat(ur5wrist2_rotmat)
ur5rgtwrist2_nodepath.setColor(.5, .5, .5, 1)
ur5rgtwrist2_nodepath.reparentTo(ur5mnp)
#
# wrist3 egg coordinates is rotated 90 around z retracted 72.33752-81.82489 to fit parameters
ur5wrist3_model.instanceTo(ur5rgtwrist3_nodepath)
ur5wrist3_rotmat = pg.cvtMat4(ur5dual.rgtarm[6]['rotmat'],
ur5dual.rgtarm[6]['linkpos'])
ur5rgtwrist3_nodepath.setMat(ur5wrist3_rotmat)
ur5rgtwrist3_nodepath.setColor(.5, .5, .5, 1)
ur5rgtwrist3_nodepath.reparentTo(ur5mnp)
# lft
ur5lftbase_nodepath = NodePath("ur5lftbase")
ur5lftupperarm_nodepath = NodePath("ur5lftupperarm")
ur5lftshoulder_nodepath = NodePath("ur5lftshoulder")
ur5lftforearm_nodepath = NodePath("ur5lftforearm")
ur5lftwrist1_nodepath = NodePath("ur5lftwrist1")
ur5lftwrist2_nodepath = NodePath("ur5lftwrist2")
ur5lftwrist3_nodepath = NodePath("ur5lftwrist3")
#
# base egg coordinates is retracted 89.2 along z to fit parameters
ur5base_model.instanceTo(ur5lftbase_nodepath)
ur5base_rotmat = pg.cvtMat4(ur5dual.lftarm[1]['inherentR'],
ur5dual.lftarm[1]['linkpos'])
ur5lftbase_nodepath.setMat(ur5base_rotmat)
ur5lftbase_nodepath.setColor(.5, .5, .5, 1)
ur5lftbase_nodepath.reparentTo(ur5mnp)
#
ur5shoulder_model.instanceTo(ur5lftshoulder_nodepath)
ur5shoulder_rotmat = pg.cvtMat4(ur5dual.lftarm[1]['rotmat'],
ur5dual.lftarm[1]['linkpos'])
ur5lftshoulder_nodepath.setMat(ur5shoulder_rotmat)
ur5lftshoulder_nodepath.setColor(.5, .7, .3, 1)
ur5lftshoulder_nodepath.reparentTo(ur5mnp)
#
ur5upperarm_model.instanceTo(ur5lftupperarm_nodepath)
ur5upperarm_rotmat = pg.cvtMat4(ur5dual.lftarm[2]['rotmat'],
ur5dual.lftarm[2]['linkpos'])
ur5lftupperarm_nodepath.setMat(ur5upperarm_rotmat)
ur5lftupperarm_nodepath.setColor(.5, .5, .5, 1)
ur5lftupperarm_nodepath.reparentTo(ur5mnp)
#
ur5forearm_model.instanceTo(ur5lftforearm_nodepath)
ur5forearm_rotmat = pg.cvtMat4(ur5dual.lftarm[3]['rotmat'],
ur5dual.lftarm[3]['linkpos'])
ur5lftforearm_nodepath.setMat(ur5forearm_rotmat)
ur5lftforearm_nodepath.setColor(.5, .5, .5, 1)
ur5lftforearm_nodepath.reparentTo(ur5mnp)
#
ur5wrist1_model.instanceTo(ur5lftwrist1_nodepath)
ur5wrist1_rotmat = pg.cvtMat4(ur5dual.lftarm[4]['rotmat'],
ur5dual.lftarm[4]['linkpos'])
ur5lftwrist1_nodepath.setMat(ur5wrist1_rotmat)
ur5lftwrist1_nodepath.setColor(.5, .7, .3, 1)
ur5lftwrist1_nodepath.reparentTo(ur5mnp)
#
ur5wrist2_model.instanceTo(ur5lftwrist2_nodepath)
ur5wrist2_rotmat = pg.cvtMat4(ur5dual.lftarm[5]['rotmat'],
ur5dual.lftarm[5]['linkpos'])
ur5lftwrist2_nodepath.setMat(ur5wrist2_rotmat)
ur5lftwrist2_nodepath.setColor(.5, .5, .5, 1)
ur5lftwrist2_nodepath.reparentTo(ur5mnp)
#
ur5wrist3_model.instanceTo(ur5lftwrist3_nodepath)
ur5wrist3_rotmat = pg.cvtMat4(ur5dual.lftarm[6]['rotmat'],
ur5dual.lftarm[6]['linkpos'])
ur5lftwrist3_nodepath.setMat(ur5wrist3_rotmat)
ur5lftwrist3_nodepath.setColor(.5, .5, .5, 1)
ur5lftwrist3_nodepath.reparentTo(ur5mnp)
# rgthnd
ur5robotrgthnd = handpkg.newHand('rgt')
ur5robotrgtarmljend_rotmat = pg.cvtMat4(ur5dual.rgtarm[6]['rotmat'],
ur5dual.rgtarm[6]['linkpos'])
pg.plotAxisSelf(ur5mnp, ur5dual.rgtarm[6]['linkend'],
ur5robotrgtarmljend_rotmat)
ur5robotrgthnd.setMat(ur5robotrgtarmljend_rotmat)
ur5robotrgthnd.reparentTo(ur5mnp)
if jawwidthrgt is not None:
ur5robotrgthnd.setJawwidth(jawwidthrgt)
# lfthnd
ur5robotlfthnd = handpkg.newHand('lft')
ur5robotlftarmljend_rotmat = pg.cvtMat4(ur5dual.lftarm[6]['rotmat'],
ur5dual.lftarm[6]['linkpos'])
pg.plotAxisSelf(ur5mnp, ur5dual.lftarm[6]['linkend'],
ur5robotlftarmljend_rotmat)
ur5robotlfthnd.setMat(ur5robotlftarmljend_rotmat)
ur5robotlfthnd.reparentTo(ur5mnp)
if jawwidthlft is not None:
ur5robotlfthnd.setJawwidth(jawwidthlft)
return [[
ur5rgtbase_nodepath, ur5rgtshoulder_nodepath, ur5rgtupperarm_nodepath,
ur5rgtforearm_nodepath, ur5rgtwrist1_nodepath, ur5rgtwrist2_nodepath,
ur5rgtwrist3_nodepath, ur5robotrgthnd.handnp
],
[
ur5lftbase_nodepath, ur5lftshoulder_nodepath,
ur5lftupperarm_nodepath, ur5lftforearm_nodepath,
ur5lftwrist1_nodepath, ur5lftwrist2_nodepath,
ur5lftwrist3_nodepath, ur5robotlfthnd.handnp
], []]
def genmnp(self, nxtrobot, jawwidthrgt = None, jawwidthlft = None):
"""
generate a panda3d nodepath for the nxtrobot
mnp indicates this function generates a mesh nodepath
:param nxtrobot: the nxtrobot object, see nxtrobot.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20180109
"""
identmat4 = Mat4.identMat()
# body
nxtwaist_rotmat = pg.cvtMat4(nxtrobot.base['rotmat'])
self.__nxtwaist_nodepath.setMat(nxtwaist_rotmat)
# rgtarm
nxtrgtarmlj0_rotmat = pg.cvtMat4(nxtrobot.rgtarm[1]['rotmat'], nxtrobot.rgtarm[1]['linkpos'])
self.__nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat)
nxtrgtarmlj1_rotmat = pg.cvtMat4(nxtrobot.rgtarm[2]['rotmat'], nxtrobot.rgtarm[2]['linkpos'])
self.__nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat)
nxtrgtarmlj2_rotmat = pg.cvtMat4(nxtrobot.rgtarm[3]['rotmat'], nxtrobot.rgtarm[3]['linkpos'])
self.__nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat)
nxtrgtarmlj3_rotmat = pg.cvtMat4(nxtrobot.rgtarm[4]['rotmat'], nxtrobot.rgtarm[4]['linkpos'])
self.__nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat)
nxtrgtarmlj4_rotmat = pg.cvtMat4(nxtrobot.rgtarm[5]['rotmat'], nxtrobot.rgtarm[5]['linkpos'])
self.__nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat)
# lftarm
nxtlftarmlj0_rotmat = pg.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos'])
self.__nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat)
nxtlftarmlj1_rotmat = pg.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos'])
self.__nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat)
nxtlftarmlj2_rotmat = pg.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos'])
self.__nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat)
nxtlftarmlj3_rotmat = pg.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos'])
self.__nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat)
nxtlftarmlj4_rotmat = pg.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos'])
self.__nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat)
# rgthand
nxtlftarmlj5_rotmat = pg.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos'])
self.nxtrgthnd.setMat(nxtlftarmlj5_rotmat)
pg.plotAxisSelf(self.__nxtmnp, nxtrobot.rgtarm[6]['linkend'], nxtlftarmlj5_rotmat)
if jawwidthrgt is not None:
self.nxtrgthnd.setJawwidth(jawwidthrgt)
# lfthand
nxtlftarmlj5_rotmat = pg.cvtMat4(nxtrobot.lftarm[6]['rotmat'], nxtrobot.lftarm[6]['linkpos'])
self.nxtlfthnd.setMat(nxtlftarmlj5_rotmat)
pg.plotAxisSelf(self.__nxtmnp, nxtrobot.lftarm[6]['linkend'], nxtlftarmlj5_rotmat)
if jawwidthlft is not None:
self.nxtlfthnd.setJawwidth(jawwidthlft)
return copy.deepcopy(self.__nxtmnp)
def genmnp(ur3dual, handpkg, jawwidthrgt = None, jawwidthlft = None):
"""
generate a panda3d nodepath for the ur3dual
mnp indicates this function generates a mesh nodepath
:param ur3dual:
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20161202
"""
ur3mnp = NodePath("ur3dualmnp")
this_dir, this_filename = os.path.split(__file__)
# chest0-2, head1 (neck is not plotted)
ur3base_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "base.egg"))
ur3upperarm_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "upperarm.egg"))
ur3shoulder_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "shoulder.egg"))
ur3forearm_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "forearm.egg"))
ur3wrist1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "wrist1.egg"))
ur3wrist2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "wrist2.egg"))
ur3wrist3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "wrist3.egg"))
ur3base_model = loader.loadModel(ur3base_filepath)
ur3upperarm_model = loader.loadModel(ur3upperarm_filepath)
ur3shoulder_model = loader.loadModel(ur3shoulder_filepath)
ur3forearm_model = loader.loadModel(ur3forearm_filepath)
ur3wrist1_model = loader.loadModel(ur3wrist1_filepath)
ur3wrist2_model = loader.loadModel(ur3wrist2_filepath)
ur3wrist3_model = loader.loadModel(ur3wrist3_filepath)
# rgt
ur3rgtbase_nodepath = NodePath("ur3rgtbase")
ur3rgtshoulder_nodepath = NodePath("ur3rgtshoulder")
ur3rgtupperarm_nodepath = NodePath("ur3rgtupperarm")
ur3rgtforearm_nodepath = NodePath("ur3rgtforearm")
ur3rgtwrist1_nodepath = NodePath("ur3rgtwrist1")
ur3rgtwrist2_nodepath = NodePath("ur3rgtwrist2")
ur3rgtwrist3_nodepath = NodePath("ur3rgtwrist3")
#
# base egg coordinates is retracted 89.2 along z to fit parameters
ur3base_model.instanceTo(ur3rgtbase_nodepath)
ur3base_rotmat = pg.cvtMat4(ur3dual.rgtarm[1]['inherentR'], ur3dual.rgtarm[1]['linkpos'])
ur3rgtbase_nodepath.setMat(ur3base_rotmat)
ur3rgtbase_nodepath.setColor(.5,.5,.5,1)
ur3rgtbase_nodepath.reparentTo(ur3mnp)
#
ur3shoulder_model.instanceTo(ur3rgtshoulder_nodepath)
ur3shoulder_rotmat = pg.cvtMat4(ur3dual.rgtarm[1]['rotmat'], ur3dual.rgtarm[1]['linkpos'])
ur3rgtshoulder_nodepath.setMat(ur3shoulder_rotmat)
ur3rgtshoulder_nodepath.setColor(.5,.7,.3,1)
ur3rgtshoulder_nodepath.reparentTo(ur3mnp)
#
ur3upperarm_model.instanceTo(ur3rgtupperarm_nodepath)
ur3upperarm_rotmat = pg.cvtMat4(ur3dual.rgtarm[2]['rotmat'], ur3dual.rgtarm[2]['linkpos'])
ur3rgtupperarm_nodepath.setMat(ur3upperarm_rotmat)
ur3rgtupperarm_nodepath.setColor(.5,.5,.5,1)
ur3rgtupperarm_nodepath.reparentTo(ur3mnp)
#
ur3forearm_model.instanceTo(ur3rgtforearm_nodepath)
ur3forearm_rotmat = pg.cvtMat4(ur3dual.rgtarm[3]['rotmat'], ur3dual.rgtarm[3]['linkpos'])
ur3rgtforearm_nodepath.setMat(ur3forearm_rotmat)
ur3rgtforearm_nodepath.setColor(.5,.5,.5,1)
ur3rgtforearm_nodepath.reparentTo(ur3mnp)
#
ur3wrist1_model.instanceTo(ur3rgtwrist1_nodepath)
ur3wrist1_rotmat = pg.cvtMat4(ur3dual.rgtarm[4]['rotmat'], ur3dual.rgtarm[4]['linkpos'])
ur3rgtwrist1_nodepath.setMat(ur3wrist1_rotmat)
ur3rgtwrist1_nodepath.setColor(.5,.7,.3,1)
ur3rgtwrist1_nodepath.reparentTo(ur3mnp)
#
ur3wrist2_model.instanceTo(ur3rgtwrist2_nodepath)
ur3wrist2_rotmat = pg.cvtMat4(ur3dual.rgtarm[5]['rotmat'], ur3dual.rgtarm[5]['linkpos'])
ur3rgtwrist2_nodepath.setMat(ur3wrist2_rotmat)
ur3rgtwrist2_nodepath.setColor(.5,.5,.5,1)
ur3rgtwrist2_nodepath.reparentTo(ur3mnp)
#
# wrist3 egg coordinates is rotated 90 around z retracted 72.33752-81.82489 to fit parameters
ur3wrist3_model.instanceTo(ur3rgtwrist3_nodepath)
ur3wrist3_rotmat = pg.cvtMat4(ur3dual.rgtarm[6]['rotmat'], ur3dual.rgtarm[6]['linkpos'])
ur3rgtwrist3_nodepath.setMat(ur3wrist3_rotmat)
ur3rgtwrist3_nodepath.setColor(.5,.5,.5,1)
ur3rgtwrist3_nodepath.reparentTo(ur3mnp)
# lft
ur3lftbase_nodepath = NodePath("ur3lftbase")
ur3lftupperarm_nodepath = NodePath("ur3lftupperarm")
ur3lftshoulder_nodepath = NodePath("ur3lftshoulder")
ur3lftforearm_nodepath = NodePath("ur3lftforearm")
ur3lftwrist1_nodepath = NodePath("ur3lftwrist1")
ur3lftwrist2_nodepath = NodePath("ur3lftwrist2")
ur3lftwrist3_nodepath = NodePath("ur3lftwrist3")
#
# base egg coordinates is retracted 89.2 along z to fit parameters
ur3base_model.instanceTo(ur3lftbase_nodepath)
ur3base_rotmat = pg.cvtMat4(ur3dual.lftarm[1]['inherentR'], ur3dual.lftarm[1]['linkpos'])
ur3lftbase_nodepath.setMat(ur3base_rotmat)
ur3lftbase_nodepath.setColor(.5,.5,.5,1)
ur3lftbase_nodepath.reparentTo(ur3mnp)
#
ur3shoulder_model.instanceTo(ur3lftshoulder_nodepath)
ur3shoulder_rotmat = pg.cvtMat4(ur3dual.lftarm[1]['rotmat'], ur3dual.lftarm[1]['linkpos'])
ur3lftshoulder_nodepath.setMat(ur3shoulder_rotmat)
ur3lftshoulder_nodepath.setColor(.5,.7,.3,1)
ur3lftshoulder_nodepath.reparentTo(ur3mnp)
#
ur3upperarm_model.instanceTo(ur3lftupperarm_nodepath)
ur3upperarm_rotmat = pg.cvtMat4(ur3dual.lftarm[2]['rotmat'], ur3dual.lftarm[2]['linkpos'])
ur3lftupperarm_nodepath.setMat(ur3upperarm_rotmat)
ur3lftupperarm_nodepath.setColor(.5,.5,.5,1)
ur3lftupperarm_nodepath.reparentTo(ur3mnp)
#
ur3forearm_model.instanceTo(ur3lftforearm_nodepath)
ur3forearm_rotmat = pg.cvtMat4(ur3dual.lftarm[3]['rotmat'], ur3dual.lftarm[3]['linkpos'])
ur3lftforearm_nodepath.setMat(ur3forearm_rotmat)
ur3lftforearm_nodepath.setColor(.5,.5,.5,1)
ur3lftforearm_nodepath.reparentTo(ur3mnp)
#
ur3wrist1_model.instanceTo(ur3lftwrist1_nodepath)
ur3wrist1_rotmat = pg.cvtMat4(ur3dual.lftarm[4]['rotmat'], ur3dual.lftarm[4]['linkpos'])
ur3lftwrist1_nodepath.setMat(ur3wrist1_rotmat)
ur3lftwrist1_nodepath.setColor(.5,.7,.3,1)
ur3lftwrist1_nodepath.reparentTo(ur3mnp)
#
ur3wrist2_model.instanceTo(ur3lftwrist2_nodepath)
ur3wrist2_rotmat = pg.cvtMat4(ur3dual.lftarm[5]['rotmat'], ur3dual.lftarm[5]['linkpos'])
ur3lftwrist2_nodepath.setMat(ur3wrist2_rotmat)
ur3lftwrist2_nodepath.setColor(.5,.5,.5,1)
ur3lftwrist2_nodepath.reparentTo(ur3mnp)
#
ur3wrist3_model.instanceTo(ur3lftwrist3_nodepath)
ur3wrist3_rotmat = pg.cvtMat4(ur3dual.lftarm[6]['rotmat'], ur3dual.lftarm[6]['linkpos'])
ur3lftwrist3_nodepath.setMat(ur3wrist3_rotmat)
ur3lftwrist3_nodepath.setColor(.5,.5,.5,1)
ur3lftwrist3_nodepath.reparentTo(ur3mnp)
# rgthnd
ur3robotrgthnd = handpkg.newHand('rgt', ftsensoroffset = -50)
ur3robotrgtarmljend_rotmat = pg.cvtMat4(ur3dual.rgtarm[6]['rotmat'], ur3dual.rgtarm[6]['linkpos'])
# pg.plotAxisSelf(ur3mnp, ur3dual.rgtarm[6]['linkend'], ur3robotrgtarmljend_rotmat)
ur3robotrgthnd.setMat(ur3robotrgtarmljend_rotmat)
ur3robotrgthnd.reparentTo(ur3mnp)
if jawwidthrgt is not None:
ur3robotrgthnd.setJawwidth(jawwidthrgt)
# lfthnd
ur3robotlfthnd = handpkg.newHand('lft', ftsensoroffset = -50)
ur3robotlftarmljend_rotmat = pg.cvtMat4(ur3dual.lftarm[6]['rotmat'], ur3dual.lftarm[6]['linkpos'])
# pg.plotAxisSelf(ur3mnp, ur3dual.lftarm[6]['linkend'], ur3robotlftarmljend_rotmat)
ur3robotlfthnd.setMat(ur3robotlftarmljend_rotmat)
ur3robotlfthnd.reparentTo(ur3mnp)
if jawwidthlft is not None:
ur3robotlfthnd.setJawwidth(jawwidthlft)
return ur3mnp
def genmnp_nm(self,hrp5nrobot, jawwidthrgt = None, jawwidthlft = None, plotcolor=[.5,.5,.5,.3]):
"""
generate a panda3d nodepath for the hrp5robot
mnp indicates this function generates a mesh nodepath
nm indicates the robots include no material
since I didn't remove the materials on the hrp5 model,
this function is essentially the same as genmnp_nm
:param hrp5robot: the Hrp5Robot object, see hrp5.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20170613
"""
identmat4 = Mat4.identMat()
# body
self.__hrp5nbody_nodepath.setMat(identmat4)
self.__hrp5nbody_nodepath.setZ(self.__hrp5nmnp, 0)
# chest 0
self.__hrp5nchest0_nodepath.setMat(identmat4)
self.__hrp5nchest0_nodepath.setZ(self.__hrp5nmnp, 274)
# chest 1
self.__hrp5nchest1_nodepath.setMat(identmat4)
self.__hrp5nchest1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# chest 2
hrp5nchest2_rotmat = pg.cvtMat4(hrp5nrobot.base['rotmat'])
self.__hrp5nchest2_nodepath.setMat(hrp5nchest2_rotmat)
self.__hrp5nchest2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# head 0
self.__hrp5nhead0_nodepath.setMat(identmat4)
self.__hrp5nhead0_nodepath.setH(hrp5nrobot.initjnts[0])
self.__hrp5nhead0_nodepath.setX(32)
self.__hrp5nhead0_nodepath.setZ(521)
# head 1
self.__hrp5nhead1_nodepath.setP(hrp5nrobot.initjnts[1])
self.__hrp5nhead1_nodepath.setX(40)
self.__hrp5nhead1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# rgtarm 0
hrp5nrgtarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[1]['rotmat'], hrp5nrobot.rgtarm[1]['linkpos'])
self.__hrp5nrgtarmlj0_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj0_rotmat)
self.__hrp5nrgtarmlj0_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# rgtarm 1
hrp5nrgtarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[2]['rotmat'], hrp5nrobot.rgtarm[2]['linkpos'])
self.__hrp5nrgtarmlj1_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj1_rotmat)
self.__hrp5nrgtarmlj1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# rgtarm 2
hrp5nrgtarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[3]['rotmat'], hrp5nrobot.rgtarm[3]['linkpos'])
self.__hrp5nrgtarmlj2_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj2_rotmat)
self.__hrp5nrgtarmlj2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# rgtarm 3
hrp5nrgtarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[4]['rotmat'], hrp5nrobot.rgtarm[4]['linkpos'])
self.__hrp5nrgtarmlj3_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj3_rotmat)
self.__hrp5nrgtarmlj3_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# rgtarm 4
hrp5nrgtarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[5]['rotmat'], hrp5nrobot.rgtarm[5]['linkpos'])
self.__hrp5nrgtarmlj4_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj4_rotmat)
self.__hrp5nrgtarmlj4_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# rgtarm 5
hrp5nrgtarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[6]['rotmat'], hrp5nrobot.rgtarm[6]['linkpos'])
self.__hrp5nrgtarmlj5_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj5_rotmat)
self.__hrp5nrgtarmlj5_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# rgtarm 6
hrp5nrgtarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[7]['rotmat'], hrp5nrobot.rgtarm[7]['linkpos'])
self.__hrp5nrgtarmlj6_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj6_rotmat)
self.__hrp5nrgtarmlj6_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# rgtarm 7
hrp5nrgtarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[8]['rotmat'], hrp5nrobot.rgtarm[8]['linkpos'])
self.__hrp5nrgtarmlj7_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj7_rotmat)
self.__hrp5nrgtarmlj7_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# lftarm 0
hrp5nlftarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[1]['rotmat'], hrp5nrobot.lftarm[1]['linkpos'])
self.__hrp5nlftarmlj0_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj0_rotmat)
self.__hrp5nlftarmlj0_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# lftarm 1
hrp5nlftarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[2]['rotmat'], hrp5nrobot.lftarm[2]['linkpos'])
self.__hrp5nlftarmlj1_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj1_rotmat)
self.__hrp5nlftarmlj1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# lftarm 2
hrp5nlftarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[3]['rotmat'], hrp5nrobot.lftarm[3]['linkpos'])
self.__hrp5nlftarmlj2_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj2_rotmat)
self.__hrp5nlftarmlj2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# lftarm 3
hrp5nlftarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[4]['rotmat'], hrp5nrobot.lftarm[4]['linkpos'])
self.__hrp5nlftarmlj3_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj3_rotmat)
self.__hrp5nlftarmlj3_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# lftarm 4
hrp5nlftarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[5]['rotmat'], hrp5nrobot.lftarm[5]['linkpos'])
self.__hrp5nlftarmlj4_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj4_rotmat)
self.__hrp5nlftarmlj4_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# lftarm 5
hrp5nlftarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[6]['rotmat'], hrp5nrobot.lftarm[6]['linkpos'])
self.__hrp5nlftarmlj5_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj5_rotmat)
self.__hrp5nlftarmlj5_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# lftarm 6
hrp5nlftarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[7]['rotmat'], hrp5nrobot.lftarm[7]['linkpos'])
self.__hrp5nlftarmlj6_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj6_rotmat)
self.__hrp5nlftarmlj6_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# lftarm 7
hrp5nlftarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[8]['rotmat'], hrp5nrobot.lftarm[8]['linkpos'])
self.__hrp5nlftarmlj7_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj7_rotmat)
self.__hrp5nlftarmlj7_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3])
# rgthand
hrp5nrobotrgtarmlj9_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[9]['rotmat'], hrp5nrobot.rgtarm[9]['linkpos'])
self.hrp5nrobotrgthnd.setMat(self.__hrp5nmnp, hrp5nrobotrgtarmlj9_rotmat)
if jawwidthrgt is not None:
self.hrp5nrobotrgthnd.setJawwidth(jawwidthrgt)
# lfthand
hrp5nrobotlftarmlj9_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[9]['rotmat'], hrp5nrobot.lftarm[9]['linkpos'])
self.hrp5nrobotlfthnd.setMat(self.__hrp5nmnp, hrp5nrobotlftarmlj9_rotmat)
if jawwidthlft is not None:
self.hrp5nrobotlfthnd.setJawwidth(jawwidthlft)
return copy.deepcopy(self.__hrp5nmnp)
def genmnp(self,
robot,
jawwidthrgt=0,
jawwidthlft=0,
toggleendcoord=False,
togglejntscoord=False,
name='robotmesh'):
"""
generate a panda3d nodepath for the hrp5robot
mnp indicates this function generates a mesh nodepath
:param robot: the Hrp5Robot object, see hrp5.py
: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: 20180203
"""
robotmesh_nodepath = NodePath(name)
# body
hrp5pbody_rotmat = pg.cvtMat4(robot.rgtarm[0]['rotmat'],
robot.rgtarm[0]['linkpos'])
self.__hrp5pbody_nodepath.reparentTo(robotmesh_nodepath)
#ADDED
# self.__hrp5pchest0_nodepath.setMat(hrp5pbody_rotmat)
# self.__hrp5pchest1_nodepath.setMat(hrp5pbody_rotmat)
# ADDED
self.__hrp5pchest2_nodepath.setMat(hrp5pbody_rotmat)
# self.__hrp5phead0_nodepath.setMat(hrp5pbody_rotmat)
# self.__hrp5phead1_nodepath.setMat(hrp5pbody_rotmat)
self.__hrp5pbody_nodepath.setColor(0.2, 0.2, 0.2, 1)
# rgtarm 0
hrp5prgtarmlj0_rotmat = pg.cvtMat4(robot.rgtarm[1]['rotmat'],
robot.rgtarm[1]['linkpos'])
self.__hrp5prgtarmlj0_nodepath.setMat(hrp5prgtarmlj0_rotmat)
self.__hrp5prgtarmlj0_nodepath.setColor(0.2, 0.2, 0.2, 1)
# rgtarm 1
hrp5prgtarmlj1_rotmat = pg.cvtMat4(robot.rgtarm[2]['rotmat'],
robot.rgtarm[2]['linkpos'])
self.__hrp5prgtarmlj1_nodepath.setMat(hrp5prgtarmlj1_rotmat)
self.__hrp5prgtarmlj1_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 2
hrp5prgtarmlj2_rotmat = pg.cvtMat4(robot.rgtarm[3]['rotmat'],
robot.rgtarm[3]['linkpos'])
self.__hrp5prgtarmlj2_nodepath.setMat(hrp5prgtarmlj2_rotmat)
self.__hrp5prgtarmlj2_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 3
hrp5prgtarmlj3_rotmat = pg.cvtMat4(robot.rgtarm[4]['rotmat'],
robot.rgtarm[4]['linkpos'])
self.__hrp5prgtarmlj3_nodepath.setMat(hrp5prgtarmlj3_rotmat)
self.__hrp5prgtarmlj3_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 4
hrp5prgtarmlj4_rotmat = pg.cvtMat4(robot.rgtarm[5]['rotmat'],
robot.rgtarm[5]['linkpos'])
self.__hrp5prgtarmlj4_nodepath.setMat(hrp5prgtarmlj4_rotmat)
self.__hrp5prgtarmlj4_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 5
hrp5prgtarmlj5_rotmat = pg.cvtMat4(robot.rgtarm[6]['rotmat'],
robot.rgtarm[6]['linkpos'])
self.__hrp5prgtarmlj5_nodepath.setMat(hrp5prgtarmlj5_rotmat)
self.__hrp5prgtarmlj5_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 6
hrp5prgtarmlj6_rotmat = pg.cvtMat4(robot.rgtarm[7]['rotmat'],
robot.rgtarm[7]['linkpos'])
self.__hrp5prgtarmlj6_nodepath.setMat(hrp5prgtarmlj6_rotmat)
self.__hrp5prgtarmlj6_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 7
hrp5prgtarmlj7_rotmat = pg.cvtMat4(robot.rgtarm[8]['rotmat'],
robot.rgtarm[8]['linkpos'])
self.__hrp5prgtarmlj7_nodepath.setMat(hrp5prgtarmlj7_rotmat)
self.__hrp5prgtarmlj7_nodepath.setColor(.5, .5, .5, 1)
self.__hrp5prgtarmlj0_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5prgtarmlj1_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5prgtarmlj2_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5prgtarmlj3_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5prgtarmlj4_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5prgtarmlj5_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5prgtarmlj6_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5prgtarmlj7_nodepath.reparentTo(robotmesh_nodepath)
# lftarm 0
hrp5plftarmlj0_rotmat = pg.cvtMat4(robot.lftarm[1]['rotmat'],
robot.lftarm[1]['linkpos'])
self.__hrp5plftarmlj0_nodepath.setMat(hrp5plftarmlj0_rotmat)
self.__hrp5plftarmlj0_nodepath.setColor(0.2, 0.2, 0.2, 1)
# lftarm 1
hrp5plftarmlj1_rotmat = pg.cvtMat4(robot.lftarm[2]['rotmat'],
robot.lftarm[2]['linkpos'])
self.__hrp5plftarmlj1_nodepath.setMat(hrp5plftarmlj1_rotmat)
self.__hrp5plftarmlj1_nodepath.setColor(.5, .5, .5, 1)
# lftarm 2
hrp5plftarmlj2_rotmat = pg.cvtMat4(robot.lftarm[3]['rotmat'],
robot.lftarm[3]['linkpos'])
self.__hrp5plftarmlj2_nodepath.setMat(hrp5plftarmlj2_rotmat)
self.__hrp5plftarmlj2_nodepath.setColor(.5, .5, .5, 1)
# lftarm 3
hrp5plftarmlj3_rotmat = pg.cvtMat4(robot.lftarm[4]['rotmat'],
robot.lftarm[4]['linkpos'])
self.__hrp5plftarmlj3_nodepath.setMat(hrp5plftarmlj3_rotmat)
self.__hrp5plftarmlj3_nodepath.setColor(.5, .5, .5, 1)
# lftarm 4
hrp5plftarmlj4_rotmat = pg.cvtMat4(robot.lftarm[5]['rotmat'],
robot.lftarm[5]['linkpos'])
self.__hrp5plftarmlj4_nodepath.setMat(hrp5plftarmlj4_rotmat)
self.__hrp5plftarmlj4_nodepath.setColor(.5, .5, .5, 1)
# lftarm 5
hrp5plftarmlj5_rotmat = pg.cvtMat4(robot.lftarm[6]['rotmat'],
robot.lftarm[6]['linkpos'])
self.__hrp5plftarmlj5_nodepath.setMat(hrp5plftarmlj5_rotmat)
self.__hrp5plftarmlj5_nodepath.setColor(.5, .5, .5, 1)
# lftarm 6
hrp5plftarmlj6_rotmat = pg.cvtMat4(robot.lftarm[7]['rotmat'],
robot.lftarm[7]['linkpos'])
self.__hrp5plftarmlj6_nodepath.setMat(hrp5plftarmlj6_rotmat)
self.__hrp5plftarmlj6_nodepath.setColor(.5, .5, .5, 1)
# lftarm 7
hrp5plftarmlj7_rotmat = pg.cvtMat4(robot.lftarm[8]['rotmat'],
robot.lftarm[8]['linkpos'])
self.__hrp5plftarmlj7_nodepath.setMat(hrp5plftarmlj7_rotmat)
self.__hrp5plftarmlj7_nodepath.setColor(.5, .5, .5, 1)
self.__hrp5plftarmlj0_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5plftarmlj1_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5plftarmlj2_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5plftarmlj3_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5plftarmlj4_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5plftarmlj5_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5plftarmlj6_nodepath.reparentTo(robotmesh_nodepath)
self.__hrp5plftarmlj7_nodepath.reparentTo(robotmesh_nodepath)
# endcoord
if toggleendcoord:
self.pggen.plotAxis(robotmesh_nodepath,
pandamat4=pg.cvtMat4(
robot.rgtarm[-1]['rotmat'],
robot.rgtarm[-1]['linkend']))
self.pggen.plotAxis(robotmesh_nodepath,
pandamat4=pg.cvtMat4(
robot.lftarm[-1]['rotmat'],
robot.lftarm[-1]['linkend']))
# toggle all coord
if togglejntscoord:
for i in range(1, 8):
self.pggen.plotAxis(robotmesh_nodepath,
pandamat4=pg.cvtMat4(
robot.rgtarm[i - 1]['rotmat'],
robot.rgtarm[i]['linkpos']))
self.pggen.plotAxis(robotmesh_nodepath,
pandamat4=pg.cvtMat4(
robot.lftarm[i - 1]['rotmat'],
robot.lftarm[i]['linkpos']))
if self.rgthnd is not None:
self.rgthnd.setMat(
pg.cvtMat4(robot.rgtarm[-1]['rotmat'],
robot.rgtarm[-1]['linkpos']))
self.rgthnd.setJawwidth(jawwidthrgt)
self.rgthnd.reparentTo(robotmesh_nodepath)
if self.lfthnd is not None:
self.lfthnd.setMat(
pg.cvtMat4(robot.lftarm[-1]['rotmat'],
robot.lftarm[-1]['linkpos']))
self.lfthnd.setJawwidth(jawwidthlft)
self.lfthnd.reparentTo(robotmesh_nodepath)
robotlegmesh_nodepath = self.genlegmnp(robot,
toggleendcoord=toggleendcoord,
togglejntscoord=togglejntscoord)
robotlegmesh_nodepath.reparentTo(robotmesh_nodepath)
return copy.deepcopy(robotmesh_nodepath)
def removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
# if self.rtq85plotlist:
# for rtq85plotnode in self.rtq85plotlist:
# rtq85plotnode.removeNode()
# self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs_precc
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs_precc[self.counter]):
for angleid in range(discretesize):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[self.counter][j][0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
tmphand = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
initmat = tmphand.getMat()
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
tmphand.setJawwidth(fgrdist)
tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmphand.setMat(pandageom.cvtMat4(rotmat) * tmphand.getMat())
axx = tmphand.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]])
tmphand.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmphand.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(self.gripcontactpairnormals_precc[self.counter][j])
# pg.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pg.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
# tmprtq85.setColor([.5, .5, .5, 1])
# tmprtq85.reparentTo(base.render)
# self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
# reset initial hand pose
tmphand.setMat(initmat)
self.counter+=1
self.counter = 0
def genlegmnp(self,
robot,
toggleendcoord=True,
togglejntscoord=False,
name='legmnp'):
"""
this function is additional to genmnp
it is an additional function to include legs
:param robot:
:return: leg mesh nodepath
author: weiwei
date: 20180203
"""
robotlegmesh_nodepath = NodePath(name)
# rgtleg 0
hrp5prgtleglj0_rotmat = pg.cvtMat4(robot.rgtleg[1]['rotmat'],
robot.rgtleg[1]['linkpos'])
self.__hrp5prgtleglj0_nodepath.setMat(hrp5prgtleglj0_rotmat)
self.__hrp5prgtleglj0_nodepath.setColor(0.2, 0.2, 0.2, 1)
# rgtleg 1
hrp5prgtleglj1_rotmat = pg.cvtMat4(robot.rgtleg[2]['rotmat'],
robot.rgtleg[2]['linkpos'])
self.__hrp5prgtleglj1_nodepath.setMat(hrp5prgtleglj1_rotmat)
self.__hrp5prgtleglj1_nodepath.setColor(.5, .5, .5, 1)
# rgtleg 2
hrp5prgtleglj2_rotmat = pg.cvtMat4(robot.rgtleg[3]['rotmat'],
robot.rgtleg[3]['linkpos'])
self.__hrp5prgtleglj2_nodepath.setMat(hrp5prgtleglj2_rotmat)
self.__hrp5prgtleglj2_nodepath.setColor(.5, .5, .5, 1)
# rgtleg 3
hrp5prgtleglj3_rotmat = pg.cvtMat4(robot.rgtleg[4]['rotmat'],
robot.rgtleg[4]['linkpos'])
self.__hrp5prgtleglj3_nodepath.setMat(hrp5prgtleglj3_rotmat)
self.__hrp5prgtleglj3_nodepath.setColor(.5, .5, .5, 1)
# rgtleg 4
hrp5prgtleglj4_rotmat = pg.cvtMat4(robot.rgtleg[5]['rotmat'],
robot.rgtleg[5]['linkpos'])
self.__hrp5prgtleglj4_nodepath.setMat(hrp5prgtleglj4_rotmat)
self.__hrp5prgtleglj4_nodepath.setColor(.5, .5, .5, 1)
# rgtleg 5
hrp5prgtleglj5_rotmat = pg.cvtMat4(robot.rgtleg[6]['rotmat'],
robot.rgtleg[6]['linkpos'])
self.__hrp5prgtleglj5_nodepath.setMat(hrp5prgtleglj5_rotmat)
self.__hrp5prgtleglj5_nodepath.setColor(.5, .5, .5, 1)
self.__hrp5prgtleglj0_nodepath.reparentTo(robotlegmesh_nodepath)
self.__hrp5prgtleglj1_nodepath.reparentTo(robotlegmesh_nodepath)
self.__hrp5prgtleglj2_nodepath.reparentTo(robotlegmesh_nodepath)
self.__hrp5prgtleglj3_nodepath.reparentTo(robotlegmesh_nodepath)
self.__hrp5prgtleglj4_nodepath.reparentTo(robotlegmesh_nodepath)
self.__hrp5prgtleglj5_nodepath.reparentTo(robotlegmesh_nodepath)
# lftleg 0
hrp5plftleglj0_rotmat = pg.cvtMat4(robot.lftleg[1]['rotmat'],
robot.lftleg[1]['linkpos'])
self.__hrp5plftleglj0_nodepath.setMat(hrp5plftleglj0_rotmat)
self.__hrp5plftleglj0_nodepath.setColor(0.2, 0.2, 0.2, 1)
# lftleg 1
hrp5plftleglj1_rotmat = pg.cvtMat4(robot.lftleg[2]['rotmat'],
robot.lftleg[2]['linkpos'])
self.__hrp5plftleglj1_nodepath.setMat(hrp5plftleglj1_rotmat)
self.__hrp5plftleglj1_nodepath.setColor(.5, .5, .5, 1)
# lftleg 2
hrp5plftleglj2_rotmat = pg.cvtMat4(robot.lftleg[3]['rotmat'],
robot.lftleg[3]['linkpos'])
self.__hrp5plftleglj2_nodepath.setMat(hrp5plftleglj2_rotmat)
self.__hrp5plftleglj2_nodepath.setColor(.5, .5, .5, 1)
# lftleg 3
hrp5plftleglj3_rotmat = pg.cvtMat4(robot.lftleg[4]['rotmat'],
robot.lftleg[4]['linkpos'])
self.__hrp5plftleglj3_nodepath.setMat(hrp5plftleglj3_rotmat)
self.__hrp5plftleglj3_nodepath.setColor(.5, .5, .5, 1)
# lftleg 4
hrp5plftleglj4_rotmat = pg.cvtMat4(robot.lftleg[5]['rotmat'],
robot.lftleg[5]['linkpos'])
self.__hrp5plftleglj4_nodepath.setMat(hrp5plftleglj4_rotmat)
self.__hrp5plftleglj4_nodepath.setColor(.5, .5, .5, 1)
# lftleg 5
hrp5plftleglj5_rotmat = pg.cvtMat4(robot.lftleg[6]['rotmat'],
robot.lftleg[6]['linkpos'])
self.__hrp5plftleglj5_nodepath.setMat(hrp5plftleglj5_rotmat)
self.__hrp5plftleglj5_nodepath.setColor(.5, .5, .5, 1)
self.__hrp5plftleglj0_nodepath.reparentTo(robotlegmesh_nodepath)
self.__hrp5plftleglj1_nodepath.reparentTo(robotlegmesh_nodepath)
self.__hrp5plftleglj2_nodepath.reparentTo(robotlegmesh_nodepath)
self.__hrp5plftleglj3_nodepath.reparentTo(robotlegmesh_nodepath)
self.__hrp5plftleglj4_nodepath.reparentTo(robotlegmesh_nodepath)
self.__hrp5plftleglj5_nodepath.reparentTo(robotlegmesh_nodepath)
# endcoord
if toggleendcoord:
self.pggen.plotAxis(robotlegmesh_nodepath,
pandamat4=pg.cvtMat4(
robot.rgtleg[-1]['rotmat'],
robot.rgtleg[-1]['linkend']))
self.pggen.plotAxis(robotlegmesh_nodepath,
pandamat4=pg.cvtMat4(
robot.lftleg[-1]['rotmat'],
robot.lftleg[-1]['linkend']))
# toggle all coord
if togglejntscoord:
for i in range(1, 6):
self.pggen.plotAxis(robotlegmesh_nodepath,
pandamat4=pg.cvtMat4(
robot.rgtleg[i - 1]['rotmat'],
robot.rgtleg[i]['linkpos']))
self.pggen.plotAxis(robotlegmesh_nodepath,
pandamat4=pg.cvtMat4(
robot.lftleg[i - 1]['rotmat'],
robot.lftleg[i]['linkpos']))
return robotlegmesh_nodepath
def saveToDB(self, positionlist, gdb, discretesize=4):
"""
:param positionlist: a list of positions to place the object one the table
:param discretesize: the discretization of rotation angles around z axis
:return:
author: weiwei
date: 20161215, osaka
"""
# save discretiezed angle
sql = "SELECT * FROM angle"
result = gdb.execute(sql)
if len(result) == 0:
sql = "INSERT INTO angle(value) VALUES "
for i in range(discretesize):
sql += "(" + str(360 * i * 1.0 / discretesize) + "), "
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "Angles already set!"
# save tabletopplacements
sql = "SELECT idstartgoal FROM startgoal,freetabletopplacement,object WHERE \
startgoal.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
# 1) select the freetabletopplacement
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
result = np.asarray(result)
idfreelist = [int(x) for x in result[:, 0]]
rotmatfreelist = [dc.strToMat4(x) for x in result[:, 1]]
# 2) select the angle
sql = "SELECT angle.idangle,angle.value FROM angle"
result = np.asarray(gdb.execute(sql))
idanglelist = [int(x) for x in result[:, 0]]
anglevaluelist = [float(x) for x in result[:, 1]]
# 3) save to database
sql = "INSERT INTO startgoal(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES "
for ttoppos in positionlist:
ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2])
for idfree, rotmatfree in zip(idfreelist, rotmatfreelist):
for idangle, anglevalue in zip(idanglelist,
anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "startgoal already exist!"
print "Save to DB done!"
def genmnp(ur5dual, handpkg, jawwidthrgt = None, jawwidthlft = None):
"""
generate a panda3d nodepath for the ur5dual
mnp indicates this function generates a mesh nodepath
:param ur5dual:
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20161202
"""
ur5mnp = NodePath("ur5dualmnp")
this_dir, this_filename = os.path.split(__file__)
# chest0-2, head1 (neck is not plotted)
ur5base_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "base.egg"))
ur5upperarm_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "upperarm.egg"))
ur5shoulder_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "shoulder.egg"))
ur5forearm_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "forearm.egg"))
ur5wrist1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "wrist1.egg"))
ur5wrist2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "wrist2.egg"))
ur5wrist3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "wrist3.egg"))
ur5base_model = loader.loadModel(ur5base_filepath)
ur5upperarm_model = loader.loadModel(ur5upperarm_filepath)
ur5shoulder_model = loader.loadModel(ur5shoulder_filepath)
ur5forearm_model = loader.loadModel(ur5forearm_filepath)
ur5wrist1_model = loader.loadModel(ur5wrist1_filepath)
ur5wrist2_model = loader.loadModel(ur5wrist2_filepath)
ur5wrist3_model = loader.loadModel(ur5wrist3_filepath)
# rgt
ur5rgtbase_nodepath = NodePath("ur5rgtbase")
ur5rgtshoulder_nodepath = NodePath("ur5rgtshoulder")
ur5rgtupperarm_nodepath = NodePath("ur5rgtupperarm")
ur5rgtforearm_nodepath = NodePath("ur5rgtforearm")
ur5rgtwrist1_nodepath = NodePath("ur5rgtwrist1")
ur5rgtwrist2_nodepath = NodePath("ur5rgtwrist2")
ur5rgtwrist3_nodepath = NodePath("ur5rgtwrist3")
#
# base egg coordinates is retracted 89.2 along z to fit parameters
ur5base_model.instanceTo(ur5rgtbase_nodepath)
ur5base_rotmat = pg.cvtMat4(ur5dual.rgtarm[1]['inherentR'], ur5dual.rgtarm[1]['linkpos'])
ur5rgtbase_nodepath.setMat(ur5base_rotmat)
ur5rgtbase_nodepath.setColor(.5,.5,.5,1)
ur5rgtbase_nodepath.reparentTo(ur5mnp)
#
ur5shoulder_model.instanceTo(ur5rgtshoulder_nodepath)
ur5shoulder_rotmat = pg.cvtMat4(ur5dual.rgtarm[1]['rotmat'], ur5dual.rgtarm[1]['linkpos'])
ur5rgtshoulder_nodepath.setMat(ur5shoulder_rotmat)
ur5rgtshoulder_nodepath.setColor(.5,.7,.3,1)
ur5rgtshoulder_nodepath.reparentTo(ur5mnp)
#
ur5upperarm_model.instanceTo(ur5rgtupperarm_nodepath)
ur5upperarm_rotmat = pg.cvtMat4(ur5dual.rgtarm[2]['rotmat'], ur5dual.rgtarm[2]['linkpos'])
ur5rgtupperarm_nodepath.setMat(ur5upperarm_rotmat)
ur5rgtupperarm_nodepath.setColor(.5,.5,.5,1)
ur5rgtupperarm_nodepath.reparentTo(ur5mnp)
#
ur5forearm_model.instanceTo(ur5rgtforearm_nodepath)
ur5forearm_rotmat = pg.cvtMat4(ur5dual.rgtarm[3]['rotmat'], ur5dual.rgtarm[3]['linkpos'])
ur5rgtforearm_nodepath.setMat(ur5forearm_rotmat)
ur5rgtforearm_nodepath.setColor(.5,.5,.5,1)
ur5rgtforearm_nodepath.reparentTo(ur5mnp)
#
ur5wrist1_model.instanceTo(ur5rgtwrist1_nodepath)
ur5wrist1_rotmat = pg.cvtMat4(ur5dual.rgtarm[4]['rotmat'], ur5dual.rgtarm[4]['linkpos'])
ur5rgtwrist1_nodepath.setMat(ur5wrist1_rotmat)
ur5rgtwrist1_nodepath.setColor(.5,.7,.3,1)
ur5rgtwrist1_nodepath.reparentTo(ur5mnp)
#
ur5wrist2_model.instanceTo(ur5rgtwrist2_nodepath)
ur5wrist2_rotmat = pg.cvtMat4(ur5dual.rgtarm[5]['rotmat'], ur5dual.rgtarm[5]['linkpos'])
ur5rgtwrist2_nodepath.setMat(ur5wrist2_rotmat)
ur5rgtwrist2_nodepath.setColor(.5,.5,.5,1)
ur5rgtwrist2_nodepath.reparentTo(ur5mnp)
#
# wrist3 egg coordinates is rotated 90 around z retracted 72.33752-81.82489 to fit parameters
ur5wrist3_model.instanceTo(ur5rgtwrist3_nodepath)
ur5wrist3_rotmat = pg.cvtMat4(ur5dual.rgtarm[6]['rotmat'], ur5dual.rgtarm[6]['linkpos'])
ur5rgtwrist3_nodepath.setMat(ur5wrist3_rotmat)
ur5rgtwrist3_nodepath.setColor(.5,.5,.5,1)
ur5rgtwrist3_nodepath.reparentTo(ur5mnp)
# lft
ur5lftbase_nodepath = NodePath("ur5lftbase")
ur5lftupperarm_nodepath = NodePath("ur5lftupperarm")
ur5lftshoulder_nodepath = NodePath("ur5lftshoulder")
ur5lftforearm_nodepath = NodePath("ur5lftforearm")
ur5lftwrist1_nodepath = NodePath("ur5lftwrist1")
ur5lftwrist2_nodepath = NodePath("ur5lftwrist2")
ur5lftwrist3_nodepath = NodePath("ur5lftwrist3")
#
# base egg coordinates is retracted 89.2 along z to fit parameters
ur5base_model.instanceTo(ur5lftbase_nodepath)
ur5base_rotmat = pg.cvtMat4(ur5dual.lftarm[1]['inherentR'], ur5dual.lftarm[1]['linkpos'])
ur5lftbase_nodepath.setMat(ur5base_rotmat)
ur5lftbase_nodepath.setColor(.5,.5,.5,1)
ur5lftbase_nodepath.reparentTo(ur5mnp)
#
ur5shoulder_model.instanceTo(ur5lftshoulder_nodepath)
ur5shoulder_rotmat = pg.cvtMat4(ur5dual.lftarm[1]['rotmat'], ur5dual.lftarm[1]['linkpos'])
ur5lftshoulder_nodepath.setMat(ur5shoulder_rotmat)
ur5lftshoulder_nodepath.setColor(.5,.7,.3,1)
ur5lftshoulder_nodepath.reparentTo(ur5mnp)
#
ur5upperarm_model.instanceTo(ur5lftupperarm_nodepath)
ur5upperarm_rotmat = pg.cvtMat4(ur5dual.lftarm[2]['rotmat'], ur5dual.lftarm[2]['linkpos'])
ur5lftupperarm_nodepath.setMat(ur5upperarm_rotmat)
ur5lftupperarm_nodepath.setColor(.5,.5,.5,1)
ur5lftupperarm_nodepath.reparentTo(ur5mnp)
#
ur5forearm_model.instanceTo(ur5lftforearm_nodepath)
ur5forearm_rotmat = pg.cvtMat4(ur5dual.lftarm[3]['rotmat'], ur5dual.lftarm[3]['linkpos'])
ur5lftforearm_nodepath.setMat(ur5forearm_rotmat)
ur5lftforearm_nodepath.setColor(.5,.5,.5,1)
ur5lftforearm_nodepath.reparentTo(ur5mnp)
#
ur5wrist1_model.instanceTo(ur5lftwrist1_nodepath)
ur5wrist1_rotmat = pg.cvtMat4(ur5dual.lftarm[4]['rotmat'], ur5dual.lftarm[4]['linkpos'])
ur5lftwrist1_nodepath.setMat(ur5wrist1_rotmat)
ur5lftwrist1_nodepath.setColor(.5,.7,.3,1)
ur5lftwrist1_nodepath.reparentTo(ur5mnp)
#
ur5wrist2_model.instanceTo(ur5lftwrist2_nodepath)
ur5wrist2_rotmat = pg.cvtMat4(ur5dual.lftarm[5]['rotmat'], ur5dual.lftarm[5]['linkpos'])
ur5lftwrist2_nodepath.setMat(ur5wrist2_rotmat)
ur5lftwrist2_nodepath.setColor(.5,.5,.5,1)
ur5lftwrist2_nodepath.reparentTo(ur5mnp)
#
ur5wrist3_model.instanceTo(ur5lftwrist3_nodepath)
ur5wrist3_rotmat = pg.cvtMat4(ur5dual.lftarm[6]['rotmat'], ur5dual.lftarm[6]['linkpos'])
ur5lftwrist3_nodepath.setMat(ur5wrist3_rotmat)
ur5lftwrist3_nodepath.setColor(.5,.5,.5,1)
ur5lftwrist3_nodepath.reparentTo(ur5mnp)
# rgthnd
ur5robotrgthnd = handpkg.newHand('rgt')
ur5robotrgtarmljend_rotmat = pg.cvtMat4(ur5dual.rgtarm[6]['rotmat'], ur5dual.rgtarm[6]['linkpos'])
pg.plotAxisSelf(ur5mnp, ur5dual.rgtarm[6]['linkend'], ur5robotrgtarmljend_rotmat)
ur5robotrgthnd.setMat(ur5robotrgtarmljend_rotmat)
ur5robotrgthnd.reparentTo(ur5mnp)
if jawwidthrgt is not None:
ur5robotrgthnd.setJawwidth(jawwidthrgt)
# lfthnd
ur5robotlfthnd = handpkg.newHand('lft')
ur5robotlftarmljend_rotmat = pg.cvtMat4(ur5dual.lftarm[6]['rotmat'], ur5dual.lftarm[6]['linkpos'])
pg.plotAxisSelf(ur5mnp, ur5dual.lftarm[6]['linkend'], ur5robotlftarmljend_rotmat)
ur5robotlfthnd.setMat(ur5robotlftarmljend_rotmat)
ur5robotlfthnd.reparentTo(ur5mnp)
if jawwidthlft is not None:
ur5robotlfthnd.setJawwidth(jawwidthlft)
return ur5mnp
hrp5nrobot = Hrp5NRobot()
# hrp5nrobot.goinitpose()
import hrp5nplot
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]])
# plot goal axis
pg.plotAxisSelf(nodepath = base.render, spos = objpos, pandamat4 = pg.cvtMat4(objrot))
# 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)
# hrp5robot.movearmfkr(armjntsgoal, armid)
# hrp5plot.plotstick(base.render, hrp5robot)
# hrp5mnp = hrp5plot.genHrp5mnp(hrp5robot)
# hrp5mnp.reparentTo(base.render)
armjntsgoal = hrp5robot.numik(objpos, objrot, armid)
if armjntsgoal is not None:
hrp5robot.movearmfk6(armjntsgoal, armid)
hrp5plot.plotstick(base.render, hrp5robot)
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])
def removeHndccShow(self, base, discretesize=8):
"""
Handcc means hand collision detection
This one is developed for demonstration
This function should be called after executing removeHndcc
:param discretesize: the number of hand orientations
:return: delayTime
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
if self.rtq85plotlist:
for rtq85plotnode in self.rtq85plotlist:
rtq85plotnode.removeNode()
self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
if self.counter2 == 0:
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
self.counter2 += 1
if self.counter2 >= discretesize:
self.counter2 = 0
print str(self.counter) + "/" + str(self.facetpairs.shape[0] - 1)
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(
self.gripcontactpairs_precc[self.counter]):
if j == 0:
print j, contactpair
# for angleid in range(discretesize):
angleid = self.counter2
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[
self.counter][j][0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
# TODO setting jawwidth to 80 is enough
# since fgrpcc already detects inner collisions
tmprtq85.setJawwidth(fgrdist)
tmprtq85.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmprtq85.setMat(pandageom.cvtMat4(rotmat) * tmprtq85.getMat())
axx = tmprtq85.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array(
[axx[0], axx[1], axx[2]])
tmprtq85.setPos(
Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
self.hndbullnode = cd.genCollisionMeshMultiNp(
tmprtq85.rtq85np, base.render)
result = self.bulletworld.contactTest(self.hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmprtq85.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(
self.gripcontactpairnormals_precc[self.counter][j])
# pandageom.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pandageom.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
tmprtq85.setColor([1, 1, 1, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
else:
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))
tmprtq85.setColor([.5, 0, 0, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
def genHrp5mnp(hrp5robot, jawwidthrgt = None, jawwidthlft = None):
"""
generate a panda3d nodepath for the hrp5robot
mnp indicates this function generates a mesh nodepath
:param hrp5robot: the Hrp5Robot object, see hrp5.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20161202
"""
hrp5mnp = NodePath("hrp5mnp")
this_dir, this_filename = os.path.split(__file__)
# chest0-2, head1 (neck is not plotted)
hrp5chest0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_chest0.egg"))
hrp5chest1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_chest1.egg"))
hrp5chest2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_chest2.egg"))
hrp5head1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_head1.egg"))
hrp5chest0_model = loader.loadModel(hrp5chest0_filepath)
hrp5chest1_model = loader.loadModel(hrp5chest1_filepath)
hrp5chest2_model = loader.loadModel(hrp5chest2_filepath)
hrp5head1_model = loader.loadModel(hrp5head1_filepath)
hrp5chest0_nodepath = NodePath("hrp5chest0")
hrp5chest1_nodepath = NodePath("hrp5chest1")
hrp5chest2_nodepath = NodePath("hrp5chest2")
hrp5head1_nodepath = NodePath("hrp5head1")
hrp5chest0_model.instanceTo(hrp5chest0_nodepath)
hrp5chest0_rotmat = pg.cvtMat4(hrp5robot.base['rotmat'])
hrp5chest0_nodepath.setMat(hrp5chest0_rotmat)
hrp5chest0_nodepath.setZ(223)
hrp5chest1_model.instanceTo(hrp5chest1_nodepath)
hrp5chest1_nodepath.setColor(.7,.7,.2,1)
hrp5chest2_model.instanceTo(hrp5chest2_nodepath)
hrp5chest2_nodepath.setX(-68)
hrp5chest2_nodepath.setColor(.7,.7,.7,1)
hrp5head1_model.instanceTo(hrp5head1_nodepath)
hrp5head1_nodepath.setH(hrp5robot.initjnts[0])
hrp5head1_nodepath.setP(hrp5robot.initjnts[1])
hrp5head1_nodepath.setX(43)
hrp5head1_nodepath.setZ(440)
hrp5head1_nodepath.setColor(.5,.5,.5,1)
hrp5chest0_nodepath.reparentTo(hrp5mnp)
hrp5chest1_nodepath.reparentTo(hrp5chest0_nodepath)
hrp5chest2_nodepath.reparentTo(hrp5chest0_nodepath)
hrp5head1_nodepath.reparentTo(hrp5chest0_nodepath)
# rgtarm
hrp5rgtarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rscap.egg"))
hrp5rgtarmlj0_model = loader.loadModel(hrp5rgtarmlj0_filepath)
hrp5rgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath")
hrp5rgtarmlj0_model.instanceTo(hrp5rgtarmlj0_nodepath)
hrp5rgtarmlj0_rotmat = pg.cvtMat4(hrp5robot.rgtarm[1]['rotmat'], hrp5robot.rgtarm[1]['linkpos'])
hrp5rgtarmlj0_nodepath.setMat(hrp5rgtarmlj0_rotmat)
hrp5rgtarmlj0_nodepath.setColor(.5,.5,.5,1)
hrp5rgtarmlj0_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink0.egg"))
hrp5rgtarmlj1_model = loader.loadModel(hrp5rgtarmlj1_filepath)
hrp5rgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath")
hrp5rgtarmlj1_model.instanceTo(hrp5rgtarmlj1_nodepath)
hrp5rgtarmlj1_rotmat = pg.cvtMat4(hrp5robot.rgtarm[2]['rotmat'], hrp5robot.rgtarm[2]['linkpos'])
hrp5rgtarmlj1_nodepath.setMat(hrp5rgtarmlj1_rotmat)
hrp5rgtarmlj1_nodepath.setColor(.5,.5,.5,1)
hrp5rgtarmlj1_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink1.egg"))
hrp5rgtarmlj2_model = loader.loadModel(hrp5rgtarmlj2_filepath)
hrp5rgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath")
hrp5rgtarmlj2_model.instanceTo(hrp5rgtarmlj2_nodepath)
hrp5rgtarmlj2_rotmat = pg.cvtMat4(hrp5robot.rgtarm[3]['rotmat'], hrp5robot.rgtarm[3]['linkpos'])
hrp5rgtarmlj2_nodepath.setMat(hrp5rgtarmlj2_rotmat)
hrp5rgtarmlj2_nodepath.setColor(.5,.5,.5,1)
hrp5rgtarmlj2_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink2.egg"))
hrp5rgtarmlj3_model = loader.loadModel(hrp5rgtarmlj3_filepath)
hrp5rgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath")
hrp5rgtarmlj3_model.instanceTo(hrp5rgtarmlj3_nodepath)
hrp5rgtarmlj3_rotmat = pg.cvtMat4(hrp5robot.rgtarm[4]['rotmat'], hrp5robot.rgtarm[4]['linkpos'])
hrp5rgtarmlj3_nodepath.setMat(hrp5rgtarmlj3_rotmat)
hrp5rgtarmlj3_nodepath.setColor(.5,.5,.5,1)
hrp5rgtarmlj3_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink3.egg"))
hrp5rgtarmlj4_model = loader.loadModel(hrp5rgtarmlj4_filepath)
hrp5rgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath")
hrp5rgtarmlj4_model.instanceTo(hrp5rgtarmlj4_nodepath)
hrp5rgtarmlj4_rotmat = pg.cvtMat4(hrp5robot.rgtarm[5]['rotmat'], hrp5robot.rgtarm[5]['linkpos'])
hrp5rgtarmlj4_nodepath.setMat(hrp5rgtarmlj4_rotmat)
hrp5rgtarmlj4_nodepath.setColor(.5,.5,.5,1)
hrp5rgtarmlj4_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj5_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink4.egg"))
hrp5rgtarmlj5_model = loader.loadModel(hrp5rgtarmlj5_filepath)
hrp5rgtarmlj5_nodepath = NodePath("nxtrgtarmlj5_nodepath")
hrp5rgtarmlj5_model.instanceTo(hrp5rgtarmlj5_nodepath)
hrp5rgtarmlj5_rotmat = pg.cvtMat4(hrp5robot.rgtarm[6]['rotmat'], hrp5robot.rgtarm[6]['linkpos'])
hrp5rgtarmlj5_nodepath.setMat(hrp5rgtarmlj5_rotmat)
hrp5rgtarmlj5_nodepath.setColor(.5,.5,.5,1)
hrp5rgtarmlj5_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj6_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink5.egg"))
hrp5rgtarmlj6_model = loader.loadModel(hrp5rgtarmlj6_filepath)
hrp5rgtarmlj6_nodepath = NodePath("nxtrgtarmlj6_nodepath")
hrp5rgtarmlj6_model.instanceTo(hrp5rgtarmlj6_nodepath)
hrp5rgtarmlj6_rotmat = pg.cvtMat4(hrp5robot.rgtarm[7]['rotmat'], hrp5robot.rgtarm[7]['linkpos'])
hrp5rgtarmlj6_nodepath.setMat(hrp5rgtarmlj6_rotmat)
hrp5rgtarmlj6_nodepath.setColor(.5,.5,.5,1)
hrp5rgtarmlj6_nodepath.reparentTo(hrp5mnp)
#
hrp5rgtarmlj7_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink6.egg"))
hrp5rgtarmlj7_model = loader.loadModel(hrp5rgtarmlj7_filepath)
hrp5rgtarmlj7_nodepath = NodePath("nxtrgtarmlj7_nodepath")
hrp5rgtarmlj7_model.instanceTo(hrp5rgtarmlj7_nodepath)
hrp5rgtarmlj7_rotmat = pg.cvtMat4(hrp5robot.rgtarm[8]['rotmat'], hrp5robot.rgtarm[8]['linkpos'])
hrp5rgtarmlj7_nodepath.setMat(hrp5rgtarmlj7_rotmat)
hrp5rgtarmlj7_nodepath.setColor(.5,.5,.5,1)
hrp5rgtarmlj7_nodepath.reparentTo(hrp5mnp)
#
# # lftarm
# nxtlftarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj0.egg"))
# nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath)
# nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath")
# nxtlftarmlj0_model.instanceTo(nxtlftarmlj0_nodepath)
# nxtlftarmlj0_rotmat = pg.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos'])
# nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat)
# nxtlftarmlj0_nodepath.reparentTo(nxtmnp)
#
# nxtlftarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj1.egg"))
# nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath)
# nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath")
# nxtlftarmlj1_model.instanceTo(nxtlftarmlj1_nodepath)
# nxtlftarmlj1_rotmat = pg.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos'])
# nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat)
# nxtlftarmlj1_nodepath.reparentTo(nxtmnp)
#
# nxtlftarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj2.egg"))
# nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath)
# nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath")
# nxtlftarmlj2_model.instanceTo(nxtlftarmlj2_nodepath)
# nxtlftarmlj2_rotmat = pg.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos'])
# nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat)
# nxtlftarmlj2_nodepath.reparentTo(nxtmnp)
#
# nxtlftarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj3.egg"))
# nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath)
# nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath")
# nxtlftarmlj3_model.instanceTo(nxtlftarmlj3_nodepath)
# nxtlftarmlj3_rotmat = pg.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos'])
# nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat)
# nxtlftarmlj3_nodepath.reparentTo(nxtmnp)
#
# nxtlftarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj4.egg"))
# nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath)
# nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath")
# nxtlftarmlj4_model.instanceTo(nxtlftarmlj4_nodepath)
# nxtlftarmlj4_rotmat = pg.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos'])
# nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat)
# nxtlftarmlj4_nodepath.reparentTo(nxtmnp)
# rgthnd
hrp5robotrgthnd = rtq85.Rtq85()
hrp5robotrgtarmlj9_rotmat = pg.cvtMat4(hrp5robot.rgtarm[9]['rotmat'], hrp5robot.rgtarm[9]['linkpos'])
# pg.plotAxisSelf(hrp5mnp, hrp5robot.rgtarm[9]['linkend'], hrp5robotrgtarmlj9_rotmat)
hrp5robotrgthnd.setMat(hrp5robotrgtarmlj9_rotmat)
hrp5robotrgthnd.reparentTo(hrp5mnp)
if jawwidthrgt is not None:
hrp5robotrgthnd.setJawwidth(jawwidthrgt)
# lfthnd
# hrp5robotlfthnd = rtq85.Rtq85()
# hrp5robotlftarmlj9_rotmat = pg.cvtMat4(hrp5robot.lftarm[9]['rotmat'], hrp5robot.lftarm[9]['linkpos'])
# pg.plotAxisSelf(hrp5mnp, hrp5robot.lftarm[9]['linkend'], hrp5robotlftarmlj9_rotmat)
# hrp5robotlfthnd.setMat(hrp5robotlftarmlj9_rotmat)
# hrp5robotlfthnd.reparentTo(hrp5mnp)
# if jawwidthlft is not None:
# hrp5robotlfthnd.setJawwidth(jawwidthlft)
return hrp5mnp
from manipulation.grip.robotiq85 import rtq85nm
from manipulation.grip.hrp5three import hrp5threenm
from nxtmesh import NxtMesh
base = pandactrl.World(camp=[1400,0,3000], lookatp=[0,0,0])
nxtrobot = NxtRobot()
nxtrobot.goinitpose()
# nxtrobot.movewaist(0)
import nxtik
objpos = np.array([200,0,300])
# objrot = np.array([[0,1,0],[1,0,0],[0,0,-1]])
objrot = np.array([[0,1,0],[0,0,-1],[-1,0,0]])
# plot goal axis
pg.plotAxisSelf(nodepath = base.render, spos = objpos, pandamat4 = pg.cvtMat4(objrot))
import nxtplot
handpkg = rtq85nm
# handpkg = hrp5threenm
# nxtrobot.movewaist(-15)
# nxtmnp = nxtplot.genmnp(nxtrobot, handpkg)
# nxtmnp.reparentTo(base.render)
nmg = NxtMesh(handpkg)
nxtmesh = nmg.genmnp(nxtrobot)
nxtmesh.reparentTo(base.render)
# nxtplot.plotstick(base.render, nxtrobot)
#
# # angle = nxtik.eurgtbik(objpos)
def genmnp_nm(nxtrobot, handpkg, plotcolor=[.5,.5,.5,.3], jawwidthrgt = None, jawwidthlft = None):
"""
generate a panda3d nodepath for the nxtrobot
mnp indicates this function generates a mesh nodepath
nm means no material
:param nxtrobot: the NxtRobot object, see nextage.py
:param plotcolor: the color of the model, alpha allowed
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20161109
"""
nxtmnp = NodePath("nxtmnp")
this_dir, this_filename = os.path.split(__file__)
# mainbody, waist, body, head (neck is not plotted)
nxtwaist_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_waist_nm.egg"))
nxtbody_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_body_nm.egg"))
nxthead_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_head_nm.egg"))
nxtwaist_model = loader.loadModel(nxtwaist_filepath)
nxtbody_model = loader.loadModel(nxtbody_filepath)
nxthead_model = loader.loadModel(nxthead_filepath)
nxtwaist_nodepath = NodePath("nxtwaist_nm")
nxtbody_nodepath = NodePath("nxtbody_nm")
nxthead_nodepath = NodePath("nxthead_nm")
nxtwaist_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxtwaist_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtbody_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxtbody_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxthead_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxthead_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtwaist_model.instanceTo(nxtwaist_nodepath)
nxtwaist_rotmat = pandageom.cvtMat4(nxtrobot.base['rotmat'])
nxtwaist_nodepath.setMat(nxtwaist_rotmat)
nxtbody_model.instanceTo(nxtbody_nodepath)
nxthead_model.instanceTo(nxthead_nodepath)
nxthead_nodepath.setH(nxtrobot.initjnts[1])
nxthead_nodepath.setP(nxtrobot.initjnts[2])
nxthead_nodepath.setZ(569.5)
nxtwaist_nodepath.reparentTo(nxtmnp)
nxtbody_nodepath.reparentTo(nxtwaist_nodepath)
nxthead_nodepath.reparentTo(nxtwaist_nodepath)
# rgtarm
nxtrgtarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj0_nm.egg"))
nxtrgtarmlj0_model = loader.loadModel(nxtrgtarmlj0_filepath)
nxtrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath")
nxtrgtarmlj0_model.instanceTo(nxtrgtarmlj0_nodepath)
nxtrgtarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[1]['rotmat'], nxtrobot.rgtarm[1]['linkpos'])
nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat)
nxtrgtarmlj0_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj0_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxtrgtarmlj0_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtrgtarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj1_nm.egg"))
nxtrgtarmlj1_model = loader.loadModel(nxtrgtarmlj1_filepath)
nxtrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath")
nxtrgtarmlj1_model.instanceTo(nxtrgtarmlj1_nodepath)
nxtrgtarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[2]['rotmat'], nxtrobot.rgtarm[2]['linkpos'])
nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat)
nxtrgtarmlj1_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj1_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxtrgtarmlj1_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtrgtarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj2_nm.egg"))
nxtrgtarmlj2_model = loader.loadModel(nxtrgtarmlj2_filepath)
nxtrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath")
nxtrgtarmlj2_model.instanceTo(nxtrgtarmlj2_nodepath)
nxtrgtarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[3]['rotmat'], nxtrobot.rgtarm[3]['linkpos'])
nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat)
nxtrgtarmlj2_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj2_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxtrgtarmlj2_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtrgtarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj3_nm.egg"))
nxtrgtarmlj3_model = loader.loadModel(nxtrgtarmlj3_filepath)
nxtrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath")
nxtrgtarmlj3_model.instanceTo(nxtrgtarmlj3_nodepath)
nxtrgtarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[4]['rotmat'], nxtrobot.rgtarm[4]['linkpos'])
nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat)
nxtrgtarmlj3_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj3_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxtrgtarmlj3_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtrgtarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj4_nm.egg"))
nxtrgtarmlj4_model = loader.loadModel(nxtrgtarmlj4_filepath)
nxtrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath")
nxtrgtarmlj4_model.instanceTo(nxtrgtarmlj4_nodepath)
nxtrgtarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[5]['rotmat'], nxtrobot.rgtarm[5]['linkpos'])
nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat)
nxtrgtarmlj4_nodepath.reparentTo(nxtmnp)
nxtrgtarmlj4_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxtrgtarmlj4_nodepath.setTransparency(TransparencyAttrib.MAlpha)
# lftarm
nxtlftarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj0_nm.egg"))
nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath)
nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath")
nxtlftarmlj0_model.instanceTo(nxtlftarmlj0_nodepath)
nxtlftarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos'])
nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat)
nxtlftarmlj0_nodepath.reparentTo(nxtmnp)
nxtlftarmlj0_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxtlftarmlj0_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtlftarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj1_nm.egg"))
nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath)
nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath")
nxtlftarmlj1_model.instanceTo(nxtlftarmlj1_nodepath)
nxtlftarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos'])
nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat)
nxtlftarmlj1_nodepath.reparentTo(nxtmnp)
nxtlftarmlj1_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxtlftarmlj1_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtlftarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj2_nm.egg"))
nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath)
nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath")
nxtlftarmlj2_model.instanceTo(nxtlftarmlj2_nodepath)
nxtlftarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos'])
nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat)
nxtlftarmlj2_nodepath.reparentTo(nxtmnp)
nxtlftarmlj2_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxtlftarmlj2_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtlftarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj3_nm.egg"))
nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath)
nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath")
nxtlftarmlj3_model.instanceTo(nxtlftarmlj3_nodepath)
nxtlftarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos'])
nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat)
nxtlftarmlj3_nodepath.reparentTo(nxtmnp)
nxtlftarmlj3_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxtlftarmlj3_nodepath.setTransparency(TransparencyAttrib.MAlpha)
nxtlftarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj4_nm.egg"))
nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath)
nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath")
nxtlftarmlj4_model.instanceTo(nxtlftarmlj4_nodepath)
nxtlftarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos'])
nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat)
nxtlftarmlj4_nodepath.reparentTo(nxtmnp)
nxtlftarmlj4_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3])
nxtlftarmlj4_nodepath.setTransparency(TransparencyAttrib.MAlpha)
# rgthnd
nxtrgthnd = handpkg.newHandNM('rgt', hndcolor=plotcolor)
nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos'])
nxtrgthnd.setMat(nxtlftarmlj5_rotmat)
nxtrgthnd.reparentTo(nxtmnp)
if jawwidthrgt is not None:
nxtrgthnd.setJawwidth(jawwidthrgt)
# lfthnd
nxtlfthnd = handpkg.newHandNM('lft', hndcolor=plotcolor)
nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[6]['rotmat'], nxtrobot.lftarm[6]['linkpos'])
nxtlfthnd.setMat(nxtlftarmlj5_rotmat)
nxtlfthnd.reparentTo(nxtmnp)
if jawwidthlft is not None:
nxtlfthnd.setJawwidth(jawwidthlft)
nxtmnp.setTransparency(TransparencyAttrib.MAlpha)
return nxtmnp
def __init__(self, jawwidth=85, hndcolor=None):
'''
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
## output
rtq85np:
the nodepath of this rtq85 hand
author: weiwei
date: 20160627
'''
self.rtq85np = NodePath("rtq85hnd")
self.handnp = self.rtq85np
self.jawwidth = jawwidth
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)
rtq85base.setPos(0, 0, 0)
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.cvtMat4(rm.rodrigues([0, 0, 1], 90)) *
rtq85base.getMat())
rtq85base.reparentTo(self.rtq85np)
self.setJawwidth(jawwidth)
self.__jawwidthopen = 85.0
self.__jawwidthclosed = 0.0
import numpy as np
import pandaplotutils.pandactrl as pc
import pandaplotutils.pandageom as pg
import utils.robotmath as rm
base = pc.World(camp=[1000, -300, 700], lookatp=[0, 0, 0], w=1000, h=1000)
pggen = pg.PandaGeomGen()
pggen.plotAxis(base.render)
pggen.plotBox(base.render,
pos=[0, 0, 0],
x=100,
y=100,
z=50,
rgba=[1, 1, .3, 1])
#
rotmat = rm.rodrigues([1, 1, 1], 30)
pandarotmat = pg.cvtMat4(rotmat, np.array([0, 0, 0]))
pggen.plotAxis(base.render, pandamat4=pandarotmat)
base.run()
def removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
# if self.rtq85plotlist:
# for rtq85plotnode in self.rtq85plotlist:
# rtq85plotnode.removeNode()
# self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
# print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs_precc
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(
self.gripcontactpairs_precc[self.counter]):
for angleid in range(discretesize):
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[
self.counter][j][0]
cctnormal1 = [
-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]
]
tmphand = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
initmat = tmphand.getMat()
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
tmphand.setJawwidth(fgrdist)
tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmphand.setMat(
pandageom.cvtMat4(rotmat) * tmphand.getMat())
axx = tmphand.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array(
[axx[0], axx[1], axx[2]])
tmphand.setPos(
Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
hndbullnode = cd.genCollisionMeshMultiNp(
tmphand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmphand.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(
self.gripcontactpairnormals_precc[self.counter][j])
# pg.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pg.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
# tmprtq85.setColor([.5, .5, .5, 1])
# tmprtq85.reparentTo(base.render)
# self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
# reset initial hand pose
tmphand.setMat(initmat)
self.counter += 1
self.counter = 0
def genmnp_nm(hrp2krobot, handpkg, jawwidthrgt = None, jawwidthlft = None):
"""
generate a panda3d nodepath for the hrp2krobot
mnp indicates this function generates a mesh nodepath
:param hrp2krobot: the Hrp5Robot object, see hrp5.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20170630
"""
hrp2kmnp = NodePath("hrp2kmnp")
this_dir, this_filename = os.path.split(__file__)
# chest0-2, head1 (neck is not plotted)
hrp2kbody_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp2kegg", "body_nm.egg"))
hrp2kchest0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp2kegg", "chest0_nm.egg"))
hrp2kchest1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp2kegg", "chest1_nm.egg"))
hrp2khead0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp2kegg", "head0_nm.egg"))
hrp2khead1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp2kegg", "head1_nm.egg"))
hrp2kbody_model = loader.loadModel(hrp2kbody_filepath)
hrp2kchest0_model = loader.loadModel(hrp2kchest0_filepath)
hrp2kchest1_model = loader.loadModel(hrp2kchest1_filepath)
hrp2khead0_model = loader.loadModel(hrp2khead0_filepath)
hrp2khead1_model = loader.loadModel(hrp2khead1_filepath)
hrp2kbody_nodepath = NodePath("hrp2kbody")
hrp2kchest0_nodepath = NodePath("hrp2kchest0")
hrp2kchest1_nodepath = NodePath("hrp2kchest1")
hrp2khead0_nodepath = NodePath("hrp2khead0")
hrp2khead1_nodepath = NodePath("hrp2khead1")
# body
hrp2kbody_model.instanceTo(hrp2kbody_nodepath)
hrp2kbody_rotmat = Mat4.identMat()
hrp2kbody_nodepath.setMat(hrp2kbody_rotmat)
hrp2kbody_nodepath.setPos(Vec3(-32, 0, -349.2))
hrp2kbody_nodepath.reparentTo(hrp2kmnp)
# chest
hrp2kchest0_model.instanceTo(hrp2kchest0_nodepath)
hrp2kchest0_rotmat = pg.cvtMat4(hrp2krobot.base['rotmat'])
hrp2kchest0_nodepath.setMat(hrp2kchest0_rotmat)
hrp2kchest0_nodepath.reparentTo(hrp2kmnp)
hrp2kchest1_model.instanceTo(hrp2kchest1_nodepath)
hrp2kchest1_rotmat = Mat4.identMat()
hrp2kchest1_nodepath.setMat(hrp2kchest1_rotmat)
hrp2kchest1_nodepath.reparentTo(hrp2kchest0_nodepath)
hrp2khead0_model.instanceTo(hrp2khead0_nodepath)
hrp2khead0_nodepath.setH(hrp2krobot.initjnts[0])
hrp2khead0_nodepath.reparentTo(hrp2kchest1_nodepath)
hrp2khead0_nodepath.setX(-7)
hrp2khead0_nodepath.setZ(297.3)
hrp2khead1_model.instanceTo(hrp2khead1_nodepath)
hrp2khead1_nodepath.setP(hrp2krobot.initjnts[1])
hrp2khead1_nodepath.reparentTo(hrp2khead0_nodepath)
# rgtarm
hrp2krgtarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "rarm0_nm.egg"))
hrp2krgtarmlj0_model = loader.loadModel(hrp2krgtarmlj0_filepath)
hrp2krgtarmlj0_nodepath = NodePath("hrp2krgtarmlj0_nodepath")
hrp2krgtarmlj0_model.instanceTo(hrp2krgtarmlj0_nodepath)
hrp2krgtarmlj0_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[1]['rotmat'], hrp2krobot.rgtarm[1]['linkpos'])
hrp2krgtarmlj0_nodepath.setMat(hrp2krgtarmlj0_rotmat)
hrp2krgtarmlj0_nodepath.reparentTo(hrp2kmnp)
# #
hrp2krgtarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "rarm1_nm.egg"))
hrp2krgtarmlj1_model = loader.loadModel(hrp2krgtarmlj1_filepath)
hrp2krgtarmlj1_nodepath = NodePath("hrp2krgtarmlj1_nodepath")
hrp2krgtarmlj1_model.instanceTo(hrp2krgtarmlj1_nodepath)
hrp2krgtarmlj1_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[2]['rotmat'], hrp2krobot.rgtarm[2]['linkpos'])
hrp2krgtarmlj1_nodepath.setMat(hrp2krgtarmlj1_rotmat)
hrp2krgtarmlj1_nodepath.reparentTo(hrp2kmnp)
# #
hrp2krgtarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "rarm2_nm.egg"))
hrp2krgtarmlj2_model = loader.loadModel(hrp2krgtarmlj2_filepath)
hrp2krgtarmlj2_nodepath = NodePath("hrp2krgtarmlj2_nodepath")
hrp2krgtarmlj2_model.instanceTo(hrp2krgtarmlj2_nodepath)
hrp2krgtarmlj2_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[3]['rotmat'], hrp2krobot.rgtarm[3]['linkpos'])
hrp2krgtarmlj2_nodepath.setMat(hrp2krgtarmlj2_rotmat)
hrp2krgtarmlj2_nodepath.reparentTo(hrp2kmnp)
# #
hrp2krgtarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "rarm3_nm.egg"))
hrp2krgtarmlj3_model = loader.loadModel(hrp2krgtarmlj3_filepath)
hrp2krgtarmlj3_nodepath = NodePath("hrp2krgtarmlj3_nodepath")
hrp2krgtarmlj3_model.instanceTo(hrp2krgtarmlj3_nodepath)
hrp2krgtarmlj3_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[4]['rotmat'], hrp2krobot.rgtarm[4]['linkpos'])
hrp2krgtarmlj3_nodepath.setMat(hrp2krgtarmlj3_rotmat)
hrp2krgtarmlj3_nodepath.reparentTo(hrp2kmnp)
# #
hrp2krgtarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "rarm4_nm.egg"))
hrp2krgtarmlj4_model = loader.loadModel(hrp2krgtarmlj4_filepath)
hrp2krgtarmlj4_nodepath = NodePath("hrp2krgtarmlj4_nodepath")
hrp2krgtarmlj4_model.instanceTo(hrp2krgtarmlj4_nodepath)
hrp2krgtarmlj4_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[5]['rotmat'], hrp2krobot.rgtarm[5]['linkpos'])
hrp2krgtarmlj4_nodepath.setMat(hrp2krgtarmlj4_rotmat)
hrp2krgtarmlj4_nodepath.reparentTo(hrp2kmnp)
# #
hrp2krgtarmlj5_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "rarm5_nm.egg"))
hrp2krgtarmlj5_model = loader.loadModel(hrp2krgtarmlj5_filepath)
hrp2krgtarmlj5_nodepath = NodePath("hrp2krgtarmlj5_nodepath")
hrp2krgtarmlj5_model.instanceTo(hrp2krgtarmlj5_nodepath)
hrp2krgtarmlj5_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[6]['rotmat'], hrp2krobot.rgtarm[6]['linkpos'])
hrp2krgtarmlj5_nodepath.setMat(hrp2krgtarmlj5_rotmat)
hrp2krgtarmlj5_nodepath.reparentTo(hrp2kmnp)
# #
hrp2krgtarmlj6_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "rarm6_nm.egg"))
hrp2krgtarmlj6_model = loader.loadModel(hrp2krgtarmlj6_filepath)
hrp2krgtarmlj6_nodepath = NodePath("hrp2krgtarmlj6_nodepath")
hrp2krgtarmlj6_model.instanceTo(hrp2krgtarmlj6_nodepath)
hrp2krgtarmlj6_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[7]['rotmat'], hrp2krobot.rgtarm[7]['linkpos'])
hrp2krgtarmlj6_nodepath.setMat(hrp2krgtarmlj6_rotmat)
hrp2krgtarmlj6_nodepath.reparentTo(hrp2kmnp)
#
# # lftarm
hrp2klftarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "larm0_nm.egg"))
hrp2klftarmlj0_model = loader.loadModel(hrp2klftarmlj0_filepath)
hrp2klftarmlj0_nodepath = NodePath("hrp2klftarmlj0_nodepath")
hrp2klftarmlj0_model.instanceTo(hrp2klftarmlj0_nodepath)
hrp2klftarmlj0_rotmat = pg.cvtMat4(hrp2krobot.lftarm[1]['rotmat'], hrp2krobot.lftarm[1]['linkpos'])
hrp2klftarmlj0_nodepath.setMat(hrp2klftarmlj0_rotmat)
hrp2klftarmlj0_nodepath.reparentTo(hrp2kmnp)
# #
hrp2klftarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "larm1_nm.egg"))
hrp2klftarmlj1_model = loader.loadModel(hrp2klftarmlj1_filepath)
hrp2klftarmlj1_nodepath = NodePath("hrp2klftarmlj1_nodepath")
hrp2klftarmlj1_model.instanceTo(hrp2klftarmlj1_nodepath)
hrp2klftarmlj1_rotmat = pg.cvtMat4(hrp2krobot.lftarm[2]['rotmat'], hrp2krobot.lftarm[2]['linkpos'])
hrp2klftarmlj1_nodepath.setMat(hrp2klftarmlj1_rotmat)
hrp2klftarmlj1_nodepath.reparentTo(hrp2kmnp)
# #
hrp2klftarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "larm2_nm.egg"))
hrp2klftarmlj2_model = loader.loadModel(hrp2klftarmlj2_filepath)
hrp2klftarmlj2_nodepath = NodePath("hrp2klftarmlj2_nodepath")
hrp2klftarmlj2_model.instanceTo(hrp2klftarmlj2_nodepath)
hrp2klftarmlj2_rotmat = pg.cvtMat4(hrp2krobot.lftarm[3]['rotmat'], hrp2krobot.lftarm[3]['linkpos'])
hrp2klftarmlj2_nodepath.setMat(hrp2klftarmlj2_rotmat)
hrp2klftarmlj2_nodepath.reparentTo(hrp2kmnp)
# #
hrp2klftarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "larm3_nm.egg"))
hrp2klftarmlj3_model = loader.loadModel(hrp2klftarmlj3_filepath)
hrp2klftarmlj3_nodepath = NodePath("hrp2klftarmlj3_nodepath")
hrp2klftarmlj3_model.instanceTo(hrp2klftarmlj3_nodepath)
hrp2klftarmlj3_rotmat = pg.cvtMat4(hrp2krobot.lftarm[4]['rotmat'], hrp2krobot.lftarm[4]['linkpos'])
hrp2klftarmlj3_nodepath.setMat(hrp2klftarmlj3_rotmat)
hrp2klftarmlj3_nodepath.reparentTo(hrp2kmnp)
# #
hrp2klftarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "larm4_nm.egg"))
hrp2klftarmlj4_model = loader.loadModel(hrp2klftarmlj4_filepath)
hrp2klftarmlj4_nodepath = NodePath("hrp2klftarmlj4_nodepath")
hrp2klftarmlj4_model.instanceTo(hrp2klftarmlj4_nodepath)
hrp2klftarmlj4_rotmat = pg.cvtMat4(hrp2krobot.lftarm[5]['rotmat'], hrp2krobot.lftarm[5]['linkpos'])
hrp2klftarmlj4_nodepath.setMat(hrp2klftarmlj4_rotmat)
hrp2klftarmlj4_nodepath.reparentTo(hrp2kmnp)
# #
hrp2klftarmlj5_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "larm5_nm.egg"))
hrp2klftarmlj5_model = loader.loadModel(hrp2klftarmlj5_filepath)
hrp2klftarmlj5_nodepath = NodePath("hrp2klftarmlj5_nodepath")
hrp2klftarmlj5_model.instanceTo(hrp2klftarmlj5_nodepath)
hrp2klftarmlj5_rotmat = pg.cvtMat4(hrp2krobot.lftarm[6]['rotmat'], hrp2krobot.lftarm[6]['linkpos'])
hrp2klftarmlj5_nodepath.setMat(hrp2klftarmlj5_rotmat)
hrp2klftarmlj5_nodepath.reparentTo(hrp2kmnp)
# #
hrp2klftarmlj6_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp2kegg", "larm6_nm.egg"))
hrp2klftarmlj6_model = loader.loadModel(hrp2klftarmlj6_filepath)
hrp2klftarmlj6_nodepath = NodePath("hrp2klftarmlj6_nodepath")
hrp2klftarmlj6_model.instanceTo(hrp2klftarmlj6_nodepath)
hrp2klftarmlj6_rotmat = pg.cvtMat4(hrp2krobot.lftarm[7]['rotmat'], hrp2krobot.lftarm[7]['linkpos'])
hrp2klftarmlj6_nodepath.setMat(hrp2klftarmlj6_rotmat)
hrp2klftarmlj6_nodepath.reparentTo(hrp2kmnp)
#
# rgthnd
hrp2krobotrgthnd = handpkg.newHandNM('rgt')
hrp2krobotrgtarmlj7_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[7]['rotmat'], hrp2krobot.rgtarm[7]['linkpos'])
# pg.plotAxisSelf(hrp2kmnp, hrp2krobot.rgtarm[9]['linkend'], hrp2krobotrgtarmlj9_rotmat)
hrp2krobotrgthnd.reparentTo(hrp2kmnp)
hrp2krobotrgthnd.setMat(hrp2krobotrgtarmlj7_rotmat)
pos = hrp2krobotrgthnd.getPos()
linklength = np.linalg.norm(hrp2krobot.rgtarm[7]['linkvec'])
hrp2krobotrgthnd.setPos(pos-hrp2krobotrgtarmlj7_rotmat.getRow3(0)*(linklength-135))
if jawwidthrgt is not None:
hrp2krobotrgthnd.setJawwidth(jawwidthrgt)
#
# lfthnd
hrp2krobotlfthnd = handpkg.newHandNM('lft')
hrp2krobotlftarmlj7_rotmat = pg.cvtMat4(hrp2krobot.lftarm[7]['rotmat'], hrp2krobot.lftarm[7]['linkpos'])
# pg.plotAxisSelf(hrp2kmnp, hrp2krobot.lftarm[9]['linkend'], hrp2krobotlftarmlj9_rotmat)
hrp2krobotlfthnd.reparentTo(hrp2kmnp)
hrp2krobotlfthnd.setMat(hrp2krobotlftarmlj7_rotmat)
pos = hrp2krobotlfthnd.getPos()
linklength = np.linalg.norm(hrp2krobot.rgtarm[7]['linkvec'])
hrp2krobotlfthnd.setPos(pos-hrp2krobotlftarmlj7_rotmat.getRow3(0)*(linklength-135))
if jawwidthlft is not None:
hrp2krobotlfthnd.setJawwidth(jawwidthlft)
hrp2kmnp.setTransparency(TransparencyAttrib.MAlpha)
return hrp2kmnp
def saveToDB(self, positionlist, gdb, discretesize=4.0):
"""
:param positionlist: a list of positions to place the object one the table
:param discretesize: the discretization of rotation angles around z axis
:return:
author: weiwei
date: 20161215, osaka
"""
# save discretiezed angle
sql = "SELECT * FROM angle"
result = gdb.execute(sql)
if len(result) == 0:
sql = "INSERT INTO angle(value) VALUES "
for i in range(discretesize):
sql += "(" + str(360 * i * 1.0 / discretesize) + "), "
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "Angles already set!"
# save tabletopplacements
sql = "SELECT idtabletopplacements FROM tabletopplacements,freetabletopplacement,object WHERE \
tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
# 1) select the freetabletopplacement
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
result = np.asarray(result)
idfreelist = [int(x) for x in result[:, 0]]
rotmatfreelist = [dc.strToMat4(x) for x in result[:, 1]]
# 2) select the angle
sql = "SELECT angle.idangle,angle.value FROM angle"
result = np.asarray(gdb.execute(sql))
idanglelist = [int(x) for x in result[:, 0]]
anglevaluelist = [float(x) for x in result[:, 1]]
# 3) save to database
sql = "INSERT INTO tabletopplacements(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES "
for ttoppos in positionlist:
ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2])
for idfree, rotmatfree in zip(idfreelist, rotmatfreelist):
for idangle, anglevalue in zip(idanglelist,
anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "Tabletopplacements already exist!"
# save tabletopgrips
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT tabletopgrips.idtabletopgrips FROM tabletopgrips,freeairgrip,object WHERE \
tabletopgrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \
freeairgrip.idobject=object.idobject AND object.name LIKE '%s' AND \
freeairgrip.idhand = %d" % (self.dbobjname, idhand)
result = gdb.execute(sql)
if len(result) == 0:
sql = "SELECT freetabletopplacement.idfreetabletopplacement \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
for idfree in result:
idfree = int(idfree[0])
sql = "SELECT tabletopplacements.idtabletopplacements, \
tabletopplacements.tabletopposition, angle.value,\
freetabletopgrip.contactpoint0, freetabletopgrip.contactpoint1, \
freetabletopgrip.contactnormal0, freetabletopgrip.contactnormal1, \
freetabletopgrip.rotmat, freetabletopgrip.jawwidth, freetabletopgrip.idfreeairgrip \
FROM tabletopplacements,freetabletopplacement,freetabletopgrip,freeairgrip,angle WHERE \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
tabletopplacements.idangle = angle.idangle AND \
freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = %d AND \
freetabletopplacement.idfreetabletopplacement = %d" % (
idhand, idfree)
result1 = gdb.execute(sql)
if len(result1) == 0:
# no grasp availalbe?
continue
# sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, \
# contactnormal1, rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES "
if len(result1) > 20000:
result1 = result1[0::int(len(result1) / 20000.0)]
result1 = np.asarray(result1)
idtabletopplacementslist = [int(x) for x in result1[:, 0]]
tabletoppositionlist = [dc.strToV3(x) for x in result1[:, 1]]
rotanglelist = [float(x) for x in result1[:, 2]]
freegripcontactpoint0list = [
dc.strToV3(x) for x in result1[:, 3]
]
freegripcontactpoint1list = [
dc.strToV3(x) for x in result1[:, 4]
]
freegripcontactnormal0list = [
dc.strToV3(x) for x in result1[:, 5]
]
freegripcontactnormal1list = [
dc.strToV3(x) for x in result1[:, 6]
]
freegriprotmatlist = [dc.strToMat4(x) for x in result1[:, 7]]
freegripjawwidthlist = [float(x) for x in result1[:, 8]]
freegripidlist = [int(x) for x in result1[:, 9]]
for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \
freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \
tabletoppositionlist, rotanglelist, freegripcontactpoint0list, freegripcontactpoint1list, \
freegripcontactnormal0list, freegripcontactnormal1list, freegriprotmatlist, freegripjawwidthlist, \
freegripidlist):
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
ttpcct0 = rotmat4.xformPoint(cct0)
ttpcct1 = rotmat4.xformPoint(cct1)
ttpcctn0 = rotmat4.xformVec(cctn0)
ttpcctn1 = rotmat4.xformVec(cctn1)
ttpgriprotmat = freegriprotmat * rotmat4
sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES \
('%s', '%s', '%s', '%s', '%s', '%s', %d, %d) " % \
(dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \
dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements)
gdb.execute(sql)
else:
print "Tabletopgrips already exist!"
print "Save to DB done!"
def genmnp_list(self, hrp5nrobot, jawwidthrgt=None, jawwidthlft=None):
"""
generate a panda3d nodepath for the hrp5robot
mnp indicates this function generates a mesh nodepath
return [[right arm mnp list], [leftarm mnp list], [body mnp list]]
where one arm mnp list is ordered from base to end-effector
this function is designed for collision detection
:param hrp5robot: the Hrp5Robot object, see hrp5.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20170613
"""
identmat4 = Mat4.identMat()
# body
self.__hrp5nbody_nodepath.setMat(identmat4)
self.__hrp5nbody_nodepath.setZ(self.__hrp5nmnp, 0)
# chest 0
self.__hrp5nchest0_nodepath.setMat(identmat4)
self.__hrp5nchest0_nodepath.setZ(self.__hrp5nmnp, 274)
# chest 1
self.__hrp5nchest1_nodepath.setMat(identmat4)
self.__hrp5nchest1_nodepath.setColor(.7, .7, .2, 1)
# chest 2
hrp5nchest2_rotmat = pg.cvtMat4(hrp5nrobot.base['rotmat'])
self.__hrp5nchest2_nodepath.setMat(hrp5nchest2_rotmat)
self.__hrp5nchest2_nodepath.setColor(.7, .7, .7, 1)
# head 0
self.__hrp5nhead0_nodepath.setMat(identmat4)
self.__hrp5nhead0_nodepath.setH(hrp5nrobot.initjnts[0])
self.__hrp5nhead0_nodepath.setX(32)
self.__hrp5nhead0_nodepath.setZ(521)
# head 1
self.__hrp5nhead1_nodepath.setP(hrp5nrobot.initjnts[1])
self.__hrp5nhead1_nodepath.setX(40)
self.__hrp5nhead1_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 0
hrp5nrgtarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[1]['rotmat'],
hrp5nrobot.rgtarm[1]['linkpos'])
self.__hrp5nrgtarmlj0_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj0_rotmat)
self.__hrp5nrgtarmlj0_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 1
hrp5nrgtarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[2]['rotmat'],
hrp5nrobot.rgtarm[2]['linkpos'])
self.__hrp5nrgtarmlj1_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj1_rotmat)
self.__hrp5nrgtarmlj1_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 2
hrp5nrgtarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[3]['rotmat'],
hrp5nrobot.rgtarm[3]['linkpos'])
self.__hrp5nrgtarmlj2_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj2_rotmat)
self.__hrp5nrgtarmlj2_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 3
hrp5nrgtarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[4]['rotmat'],
hrp5nrobot.rgtarm[4]['linkpos'])
self.__hrp5nrgtarmlj3_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj3_rotmat)
self.__hrp5nrgtarmlj3_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 4
hrp5nrgtarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[5]['rotmat'],
hrp5nrobot.rgtarm[5]['linkpos'])
self.__hrp5nrgtarmlj4_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj4_rotmat)
self.__hrp5nrgtarmlj4_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 5
hrp5nrgtarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[6]['rotmat'],
hrp5nrobot.rgtarm[6]['linkpos'])
self.__hrp5nrgtarmlj5_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj5_rotmat)
self.__hrp5nrgtarmlj5_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 6
hrp5nrgtarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[7]['rotmat'],
hrp5nrobot.rgtarm[7]['linkpos'])
self.__hrp5nrgtarmlj6_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj6_rotmat)
self.__hrp5nrgtarmlj6_nodepath.setColor(.5, .5, .5, 1)
# rgtarm 7
hrp5nrgtarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[8]['rotmat'],
hrp5nrobot.rgtarm[8]['linkpos'])
self.__hrp5nrgtarmlj7_nodepath.setMat(self.__hrp5nmnp,
hrp5nrgtarmlj7_rotmat)
self.__hrp5nrgtarmlj7_nodepath.setColor(.5, .5, .5, 1)
# lftarm 0
hrp5nlftarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[1]['rotmat'],
hrp5nrobot.lftarm[1]['linkpos'])
self.__hrp5nlftarmlj0_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj0_rotmat)
self.__hrp5nlftarmlj0_nodepath.setColor(.5, .5, .5, 1)
# lftarm 1
hrp5nlftarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[2]['rotmat'],
hrp5nrobot.lftarm[2]['linkpos'])
self.__hrp5nlftarmlj1_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj1_rotmat)
self.__hrp5nlftarmlj1_nodepath.setColor(.5, .5, .5, 1)
# lftarm 2
hrp5nlftarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[3]['rotmat'],
hrp5nrobot.lftarm[3]['linkpos'])
self.__hrp5nlftarmlj2_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj2_rotmat)
self.__hrp5nlftarmlj2_nodepath.setColor(.5, .5, .5, 1)
# lftarm 3
hrp5nlftarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[4]['rotmat'],
hrp5nrobot.lftarm[4]['linkpos'])
self.__hrp5nlftarmlj3_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj3_rotmat)
self.__hrp5nlftarmlj3_nodepath.setColor(.5, .5, .5, 1)
# lftarm 4
hrp5nlftarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[5]['rotmat'],
hrp5nrobot.lftarm[5]['linkpos'])
self.__hrp5nlftarmlj4_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj4_rotmat)
self.__hrp5nlftarmlj4_nodepath.setColor(.5, .5, .5, 1)
# lftarm 5
hrp5nlftarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[6]['rotmat'],
hrp5nrobot.lftarm[6]['linkpos'])
self.__hrp5nlftarmlj5_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj5_rotmat)
self.__hrp5nlftarmlj5_nodepath.setColor(.5, .5, .5, 1)
# lftarm 6
hrp5nlftarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[7]['rotmat'],
hrp5nrobot.lftarm[7]['linkpos'])
self.__hrp5nlftarmlj6_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj6_rotmat)
self.__hrp5nlftarmlj6_nodepath.setColor(.5, .5, .5, 1)
# lftarm 7
hrp5nlftarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[8]['rotmat'],
hrp5nrobot.lftarm[8]['linkpos'])
self.__hrp5nlftarmlj7_nodepath.setMat(self.__hrp5nmnp,
hrp5nlftarmlj7_rotmat)
self.__hrp5nlftarmlj7_nodepath.setColor(.5, .5, .5, 1)
# rgthand
hrp5nrobotrgtarmlj9_rotmat = pg.cvtMat4(
hrp5nrobot.rgtarm[9]['rotmat'], hrp5nrobot.rgtarm[9]['linkpos'])
self.hrp5nrobotrgthnd.setMat(self.__hrp5nmnp,
hrp5nrobotrgtarmlj9_rotmat)
if jawwidthrgt is not None:
self.hrp5nrobotrgthnd.setJawwidth(jawwidthrgt)
# lfthand
hrp5nrobotlftarmlj9_rotmat = pg.cvtMat4(
hrp5nrobot.lftarm[9]['rotmat'], hrp5nrobot.lftarm[9]['linkpos'])
self.hrp5nrobotlfthnd.setMat(self.__hrp5nmnp,
hrp5nrobotlftarmlj9_rotmat)
if jawwidthlft is not None:
self.hrp5nrobotlfthnd.setJawwidth(jawwidthlft)
return copy.deepcopy([
[
self.__hrp5nrgtarmlj0_nodepath, self.__hrp5nrgtarmlj1_nodepath,
self.__hrp5nrgtarmlj2_nodepath, self.__hrp5nrgtarmlj3_nodepath,
self.__hrp5nrgtarmlj4_nodepath, self.__hrp5nrgtarmlj5_nodepath,
self.__hrp5nrgtarmlj6_nodepath, self.__hrp5nrgtarmlj7_nodepath,
self.hrp5nrobotrgthnd.handnp
],
[
self.__hrp5nlftarmlj0_nodepath, self.__hrp5nlftarmlj1_nodepath,
self.__hrp5nlftarmlj2_nodepath, self.__hrp5nlftarmlj3_nodepath,
self.__hrp5nlftarmlj4_nodepath, self.__hrp5nlftarmlj5_nodepath,
self.__hrp5nlftarmlj6_nodepath, self.__hrp5nlftarmlj7_nodepath,
self.hrp5nrobotlfthnd.handnp
], [self.__hrp5nbody_nodepath]
])
