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]
import robotcon.rpc.nxtrobot.nxtrobot_client as nxtc
if __name__ == "__main__":
nxt = nxtc.NxtRobot(host="10.0.1.102:18300")
nxt.servoOff()
nxt.servoOn()
print("Joint angles: %s\n" % armjntsgoal6[ts])
if armjntsgoal6[ts] is not None:
nxtrobot.movearmfk(armjntsgoal6[ts], armname)
# nxtmesh = robotmesh.genmnp(nxtrobot, jawwidthrgt=85, jawwidthlft=0)
# nxtmesh = nmg.gensnp(nxtrobot)
# nxtmesh.reparentTo(base.render)
# dcam = loader.loadShader('dcam.sha')
# # render everything through this camera and shader
# base.render.setShader(dcam)
# # loadPrcFileData('', 'show-buffers 1')
base.pggen.plotAxis(base.render)
nxtrobot.goinitpose()
# uncomment the following commands to actuate the robot
nxtu = nxturx.NxtRobot(host = "10.0.1.102:18300")
nxtu.goInitial()
# nxtmesh = robotmesh.genmnp(nxtrobot, 85, 0)
# nxtmesh.reparentTo(base.render)
initjnts = np.array([-15.0, 0.0, -143.0, 0.0, 0.0, 0.0])
armjntsgoal6.insert(0, initjnts)
print("armjntsgoal6 is %s" % armjntsgoal6)
newpath = []
for i in range(num):
startjnts = armjntsgoal6[i]
print("start = %s" % startjnts)
goaljnts = armjntsgoal6[i+1]
print("goal = %s" % goaljnts)
planner = rrtc.RRTConnect(start=startjnts, goal=goaljnts, ctcallback=ctcallback,
starttreesamplerate=starttreesamplerate,
f1 = f.readlines()[startline: endline]
poselist = []
for string in f1:
strlist = string.strip("[").strip(" ").replace("]", "").rstrip(" ")
# s = " ".join(strlist.split()).replace(" ", ",")
s = " ".join(strlist.split())
# print(s)
list1 = eval(s)
poselist.append([list1[0], list1[1], list1[2],
list1[3], list1[4], list1[5], list1[6], list1[7], list1[8],
list1[9], list1[10], list1[11], list1[12], list1[13], list1[14]])
return poselist
if __name__ == '__main__':
nxtu = nxt.NxtRobot(host="10.0.1.114:15005")
nxtu.goInitial()
# nxtu.goOffPose()
# -------------- PREPARE TO GRIP ------------------- #
prepare_angles = readfile("Planned_Lpath_deriv2_robot.txt", 0, 25)
print(len(prepare_angles), prepare_angles)
t = []
for i in range(len(prepare_angles)):
t.append(1)
print(t)
nxtu.playPattern(prepare_angles, t)
# -------------- GRIP THE OBJ HERE!!! ------------------- #
# -------------- LIFT THE OBJ ------------------- #
lift_angles = readfile("Planned_Lpath_deriv2_robot.txt", 25, 36)
print(len(lift_angles), lift_angles)