base = pandactrl.World(camp=[1400, 1200, 0], lookatp=[0, 0, 0])
nxtrobot = NxtRobot()
nxtrobot.goinitpose()
# nxtrobot.movewaist(0)
nxtrobot.movearmfk([-1.4, 4.1, -92.6, 154.5, -87.6, -124.5])
nxtrobot.movearmfk([20, 0, -143.2, 0, 0, 0], armid='lft')
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
# base.pggen.plotAxis(nodepath = base.render, spos = objpos, pandamat4 = pg.cvtMat4(objrot))
lfthand = rtq85.Rtq85()
rgthand = rtq85.Rtq85()
nmg = NxtMesh(lfthand=lfthand, rgthand=rgthand)
nxtmesh = nmg.genmnp(nxtrobot, togglejntscoord=False)
# nxtmesh = nmg.gensnp(nxtrobot)
nxtmesh.reparentTo(base.render)
# this_dir, this_filename = os.path.split(__file__)
# objpath = os.path.join(os.path.split(this_dir)[0], "../manipulation/grip", "objects", "tool.stl")
# objmnp = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
# objmnp.reparentTo(base.render)
base.pggen.plotAxis(base.render)
base.run()
from panda3d.core import *
from panda3d.bullet import BulletSphereShape
import pandaplotutils.pandageom as pandageom
import pandaplotutils.pandactrl as pandactrl
from direct.filter.CommonFilters import CommonFilters
import manipulation.grip.robotiq85.rtq85 as rtq85
from robotsim.ur3dual import ur3dual
from robotsim.ur3dual import ur3dualmesh
import utiltools.robotmath as rm
# loadPrcFileData("", "want-directtools #t")
# loadPrcFileData("", "want-tk #t")
base = pandactrl.World(camp=[3000, 3000, 3000], lookatp=[0, 0, 1300])
rgthnd = rtq85.Rtq85()
lfthnd = rtq85.Rtq85()
ur3dualrobot = ur3dual.Ur3DualRobot(position=[0, 500, 0],
rotmat=rm.rodrigues([0, 0, 1], 70))
# goalpos = np.array([200,120,1410])
# goalrot = np.array([[1,0,0],[0,0,-1],[0,1,0]])
# goal = ur3dualrobot.numik(goalpos, goalrot, armname = 'lft')
# goal = np.array([-46.251876352032305, -41.418654079396184, 94.34173209615693, -15.917387039376576, 92.07172082393664, -25.134340575525133])
goal = np.array([
-209.13573775, -42.61307312, -283.86285256, 30.51538756, -231.85631837,
-218.24860474
])
ur3dualrobot.movearmfk(goal, armname='rgt')
# ur3dualrobot.gozeropose()
ur3dualrobotball = Ur3DualBall()
obscmlist = env.getstationaryobslist()
# for obscm in obscmlist:
# obscm.showcn()
# load obj collision model using env
objcm = env.loadobj("bunnysim.stl")
# another example -- load obj collision model independently
# this_dir, this_filename = os.path.split(__file__)
# objname = "tool_motordriver.stl"
# objpath = os.path.join(this_dir, "objects", objname)
# objcm = cm.CollisionModel(objpath)
robot = ur3dual.Ur3DualRobot()
robotball = ur3dualball.Ur3DualBall()
rgthnd = rtq85.Rtq85(jawwidth=85)
lfthnd = rtq85.Rtq85(jawwidth=85)
robotmesh = ur3dualmesh.Ur3DualMesh(rgthand=rgthnd, lfthand=lfthnd)
robotnp = robotmesh.genmnp(robot, jawwidthrgt=85.0, jawwidthlft=0.0)
# robotnp.reparentTo(base.render)
armname = 'rgt'
cdchecker = cdck.CollisionCheckerBall(robotball)
ctcallback = ctcb.CtCallback(base, robot, cdchecker, armname=armname)
smoother = sm.Smoother()
robot.goinitpose()
# robotnp = robotmesh.genmnp(robot, jawwidthrgt = 85.0, jawwidthlft=0.0)
# robotnp.reparentTo(base.render)
# base.run()
starttreesamplerate = 25
obscmlist = env.getstationaryobslist()
# for obscm in obscmlist:
# obscm.showcn()
# load obj collision model using env
objcm = env.loadobj("bunnysim.stl")
# another example -- load obj collision model independently
# this_dir, this_filename = os.path.split(__file__)
# objname = "tool_motordriver.stl"
# objpath = os.path.join(this_dir, "objects", objname)
# objcm = cm.CollisionModel(objpath)
robot = nxt.NxtRobot()
robotball = nxtball.NxtBall()
rgthnd = rtq85.Rtq85(jawwidth=85, ftsensoroffset=0)
lfthnd = rtq85.Rtq85(jawwidth=85, ftsensoroffset=0)
robotmesh = nxtmesh.NxtMesh(rgthand=rgthnd, lfthand=lfthnd)
robotnp = robotmesh.genmnp(robot, jawwidthrgt=85.0, jawwidthlft=0.0)
# robotnp.reparentTo(base.render)
armname = 'rgt'
smoother = sm.Smoother()
robot.goinitpose()
# robotnp = robotmesh.genmnp(robot, jawwidthrgt = 85.0, jawwidthlft=0.0)
# robotnp.reparentTo(base.render)
# base.run()
starttreesamplerate = 25
endtreesamplerate = 30
objstpos = np.array([354.5617667638, -256.0889005661, 1090.5])
def newHand(hndid = 'rgt', jawwidth = 85, ftsensoroffset=52):
return rtq85.Rtq85(jawwidth, ftsensoroffset)
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 direct.showbase.ShowBase import ShowBase
from panda3d.core import *
from panda3d.bullet import BulletSphereShape
import pandaplotutils.pandageom as pandageom
import pandaplotutils.pandactrl as pandactrl
from direct.filter.CommonFilters import CommonFilters
import nxt
from manipulation.grip.robotiq85 import rtq85
import nxt
import nxtmesh
# loadPrcFileData("", "want-directtools #t")
# loadPrcFileData("", "want-tk #t")
base = pandactrl.World(camp=[3000, 400, 3000], lookatp=[0, 0, 1000])
nxtrobot = nxt.NxtRobot()
nxtrobot.movewaist(70)
nxtb = NxtBall()
# bcndict = nxtb.genfullbcndict(nxtrobot)
# bcndict = nxtb.gensemibcndict(nxtrobot)
bcndict = nxtb.genholdbcndict(nxtrobot, armname="lft")
nxtb.showcn(base, bcndict)
rgthnd = rtq85.Rtq85(ftsensoroffset=0.0)
lfthnd = rtq85.Rtq85(ftsensoroffset=0.0)
nxtmeshgen = nxtmesh.NxtMesh(rgthand=rgthnd, lfthand=lfthnd)
nxtmnp = nxtmeshgen.genmnp(nxtrobot)
nxtmnp.reparentTo(base.render)
base.run()
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
base.pggen.plotAxis(nodepath=base.render,
spos=objpos,
pandamat3=base.pg.npToMat3(objrot))
armname = 'rgt'
jnts = nxtrobot.numikr(objpos, objrot, armname)
nxtrobot.movearmfkr(jnts, armname)
objpos2 = np.array([400, 200, 0])
jnts2 = nxtrobot2.numikr(objpos2, objrot, armname)
# nxtrobot2.movearmfkr(jnts2, armname)
# print(jnts)
print(jnts2)
lfthand = rtq85.Rtq85(ftsensoroffset=0)
rgthand = rtq85.Rtq85(ftsensoroffset=0)
rgthand = sck918.Sck918(ftsensoroffset=0)
nmg = NxtMesh(lfthand=lfthand, rgthand=rgthand)
nxtmesh = nmg.genmnp(nxtrobot, togglejntscoord=False)
nxtmesh2 = nmg.genmnp(nxtrobot2, togglejntscoord=False)
nxtmesh = nmg.gensnp(nxtrobot)
nxtmesh.reparentTo(base.render)
nxtmesh2.reparentTo(base.render)
# this_dir, this_filename = os.path.split(__file__)
# objpath = os.path.join(os.path.split(this_dir)[0], "../manipulation/grip", "objects", "tool.stl")
# objmnp = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
# objmnp.reparentTo(base.render)
