def __init__(self, objname, markerid=5):
self.markerid = markerid
self.objname = objname
self.gdb = db.GraspDB(database="nxt",
user="******",
password="******")
self.robot = nxtsim.NxtRobot()
self.rgthnd = sck918.Sck918(jawwidth=50,
hndcolor=(0.5, 0.5, 0.5, 1),
ftsensoroffset=0)
self.lfthnd = sck918.Sck918(jawwidth=50,
hndcolor=(0.5, 0.5, 0.5, 1),
ftsensoroffset=0)
self.robotmesh = nxtsimmesh.NxtMesh(rgthand=self.rgthnd,
lfthand=self.lfthnd)
self.robotball = nxtsimball.NxtBall()
self.robotik = nxtik
self.env = bldgfsettingnear.Env()
self.env.reparentTo(base.render)
self.objcm = self.env.loadobj(objname)
self.groove = self.env.loadobj("tenonhole_new_115.STL")
self.groove.setPos(595, 0, 973 - 76 + 75 + 80)
self.groove.setRPY(0, 90, -90)
self.groove.setColor(0, 0, 1, 0.4)
# self.groove.showLocalFrame()
self.groove.reparentTo(base.render)
self.obstaclecmlist = self.env.getstationaryobslist()
# for obstaclecdcm in self.obstaclecmlist:
# obstaclecdcm.showcn()
self.robot.goinitpose()
# self.rgtwatchpos = [400.0, -200.0, 1200.0]
# self.rgtwatchrotmat = [[0.09141122, -0.76823672, 0.63360582],
# [-0.99509199, -0.04625775, 0.08747659],
# [-0.03789355, -0.63849242, -0.76869468]]
# self.rgtwatchjs = self.robot.numik(self.rgtwatchpos, self.rgtwatchrotmat, "rgt")
#
# self.robot.movearmfk(self.rgtwatchjs, armname="rgt")
self.rbtmnp = self.robotmesh.genmnp(self.robot,
self.rgthnd.jawwidthopen,
self.lfthnd.jawwidthopen)
self.rbtmnp.reparentTo(base.render)
# uncomment the following commands to actuate the robot
self.nxtu = nxturx.NxtRobot(host="10.0.1.114:15005")
self.nxtu.goInitial()
# self.nxtu = nxturx.NxtCon()
# self.nxtu.setJointAnglesOfGroup('rarm', self.robot.initjnts[3:9], 5.0)
# self.nxtu.setJointAnglesOfGroup('larm', self.robot.initjnts[9:15], 5.0)
self.hc = hndcam.HndCam(rgtcamid=0, lftcamid=1)
# goallist, a list of 4x4 h**o numpy mat
self.goallist = []
self.objrenderlist = []
self.startobjcm = None
self.rbtmnpani = [None, None]
self.objmnpani = [None]
def __init__(self, objname, markerid=5):
self.markerid = markerid
self.objname = objname
self.gdb = db.GraspDB(database="nxt",
user="******",
password="******")
self.robot = nxtsim.NxtRobot()
self.rgthnd = rtq85nm.newHand(hndid="rgt", ftsensoroffset=0)
self.lfthnd = rtq85nm.newHand(hndid="lft", ftsensoroffset=0)
self.robotmesh = nxtsimmesh.NxtMesh(rgthand=self.rgthnd,
lfthand=self.lfthnd)
self.robotball = nxtsimball.NxtBall()
self.robotik = nxtik
self.env = bldgfsettingnear.Env()
self.env.reparentTo(base.render)
self.objcm = self.env.loadobj(objname)
self.obstaclecmlist = self.env.getstationaryobslist()
for obstaclecdcm in self.obstaclecmlist:
obstaclecdcm.showcn()
self.robot.goinitpose()
# self.rgtwatchpos = [400.0, -200.0, 1200.0]
# self.rgtwatchrotmat = [[0.09141122, -0.76823672, 0.63360582],
# [-0.99509199, -0.04625775, 0.08747659],
# [-0.03789355, -0.63849242, -0.76869468]]
# self.rgtwatchjs = self.robot.numik(self.rgtwatchpos, self.rgtwatchrotmat, "rgt")
#
# self.robot.movearmfk(self.rgtwatchjs, armname="rgt")
self.rbtmnp = self.robotmesh.genmnp(self.robot,
self.rgthnd.jawwidthopen,
self.lfthnd.jawwidthopen)
self.rbtmnp.reparentTo(base.render)
# uncomment the following commands to actuate the robot
# self.ur3u = ur3urx.Ur3DualUrx()
# self.ur3u.movejntssgl(self.robot.initjnts[3:9], 'rgt')
# self.ur3u.movejntssgl(self.robot.initjnts[9:15], 'lft')
# self.ur3u.movejntssgl(self.rgtwatchjs, 'rgt')
self.hc = cameras.HndCam(rgtcamid=0, lftcamid=1)
# goallist, a list of 4x4 h**o numpy mat
self.goallist = []
self.objrenderlist = []
self.startobjcm = None
self.rbtmnpani = [None, None]
self.objmnpani = [None]
from robotsim.ur3dual import ur3dualmesh
from robotsim.ur3dual import ur3dualball
from robotsim.nextage import nxt
from robotsim.nextage import nxtmesh
from robotsim.nextage import nxtball
from pandaplotutils import pandactrl
import bldgfsettingnear
import numpy as np
import copy
import utils.robotmath as rm
import os
import environment.collisionmodel as cm
base = pandactrl.World(camp=[2700, 300, 2700], lookatp=[0, 0, 1000])
env = bldgfsettingnear.Env(base)
env.reparentTo(base.render)
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()
pass
else:
smoother = sm.Smoother(objcm)
path = smoother.pathsmoothing(path, planner)
endtime = time.clock()
runtime = endtime - starttime
print("Run time =", runtime)
return path, runtime
if __name__=="__main__":
# base = pc.World(camp=[0, 800, 0], lookatp=[0, 0, 0], up=[0, 0, 1], fov=40, w=1920, h=1080)
base = pc.World(camp=[800, 0, 0], lookatp=[0, 0, 0], up=[0, 0, 1], fov=40, w=1920, h=1080)
# base = pc.World(camp=[200, 800, 200], lookatp=[0, 0, 0], up=[0, 0, 1], fov=40, w=1920, h=1080)
env = bldgfsettingnear.Env()
# self.env.reparentTo(base.render)
# objname = "new_LSHAPE.stl"
objname = "tenon.STL"
# objname = "twostairpeg_handle.STL"
objcm = env.loadobj(objname)
# groove = env.loadobj("new_GROOVE.stl")
groove = env.loadobj("tenonhole.STL")
# groove = env.loadobj("twostairhole_board.STL")
posG = base.pg.npToMat4(np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]),
np.array([0, 0, 0]))
setPosRPY(groove, posG)
groove.setColor(0, 0, 1, .4)
# groove.showLocalFrame()
groove.reparentTo(base.render)
