Commit d3d67b02 authored by Mathias Haage's avatar Mathias Haage

Support GBS readptp, linmove, ptpmove, locktool and unlocktool

parent f2afc4ee
...@@ -71,7 +71,7 @@ def prettyprint(array): ...@@ -71,7 +71,7 @@ def prettyprint(array):
ac = array.array[10+i] ac = array.array[10+i]
i = i + 11 i = i + 11
print "linmove x=%f y=%f z=%f c=%f b=%f a=%f s=%f ac=%f"%(x,y,z,c,b,a,s,ac) print "linmove x=%f y=%f z=%f c=%f b=%f a=%f s=%f ac=%f"%(x,y,z,c,b,a,s,ac)
elif near(c0,10) and near(c1,1) and near(c2,2): elif near(c0,10) and near(c1,2) and near(c2,1):
j1 = array.array[3+i] j1 = array.array[3+i]
j2 = array.array[4+i] j2 = array.array[4+i]
j3 = array.array[5+i] j3 = array.array[5+i]
...@@ -85,13 +85,13 @@ def prettyprint(array): ...@@ -85,13 +85,13 @@ def prettyprint(array):
elif near(c0,2) and near(c1,1) and near(c2,4): elif near(c0,2) and near(c1,1) and near(c2,4):
i = i + 3 i = i + 3
print "readlibrary" print "readlibrary"
elif near(c0,2) and near(c1,1) and near(c2,5): elif near(c0,2) and near(c1,5) and near(c2,1):
i = i + 3 i = i + 3
print "readcurrentrobotpos" print "readcurrentrobotpos"
elif near(c0,2) and near(c1,1) and near(c2,6): elif near(c0,2) and near(c1,6) and near(c2,1):
i = i + 3 i = i + 3
print "locktool" print "locktool"
elif near(c0,2) and near(c1,1) and near(c2,7): elif near(c0,2) and near(c1,7) and near(c2,1):
i = i + 3 i = i + 3
print "unlocktool" print "unlocktool"
elif near(c0,18) and near(c1,1) and near(c2,8): elif near(c0,18) and near(c1,1) and near(c2,8):
...@@ -276,11 +276,11 @@ def handle_gbs_lockspindle(req): ...@@ -276,11 +276,11 @@ def handle_gbs_lockspindle(req):
def handle_gbs_locktool(req): def handle_gbs_locktool(req):
array = Array() array = Array()
array.n = 3 array.n = 3
array.array = [2,1,6] array.array = [2,6,1]
prettyprint(array) prettyprint(array)
comm.sendArray(array) comm.sendArray(array)
ans = GBSLockToolResponse() ans = GBSLockToolResponse()
ans.err = com.receiveFloat() ans.err = comm.receiveFloat()
comm.receiveFloat() comm.receiveFloat()
return ans return ans
...@@ -313,12 +313,13 @@ def handle_gbs_naillinmove(req): ...@@ -313,12 +313,13 @@ def handle_gbs_naillinmove(req):
def handle_gbs_ptpmove(req): def handle_gbs_ptpmove(req):
array = Array() array = Array()
array.n = 11 array.n = 11
array.array = [10,1,2, req.p.j1, req.p.j2, req.p.j3, req.p.j4, req.p.j5, req.p.j6, req.s, req.ac] array.array = [10,2,1, req.p.j1, req.p.j2, req.p.j3, req.p.j4, req.p.j5, req.p.j6, req.s, req.ac]
prettyprint(array) prettyprint(array)
comm.sendArray(array) comm.sendArray(array)
ans = GBSPTPMoveResponse() ans = GBSPTPMoveResponse()
ans.err1 = comm.receiveFloat() #ans.err1 = comm.receiveFloat()
comm.receiveFloat() #comm.receiveFloat()
ans.err1 = 0
ans.err2 = comm.receiveFloat() ans.err2 = comm.receiveFloat()
comm.receiveFloat() comm.receiveFloat()
return ans return ans
...@@ -326,7 +327,7 @@ def handle_gbs_ptpmove(req): ...@@ -326,7 +327,7 @@ def handle_gbs_ptpmove(req):
def handle_gbs_readcurjointpose(req): def handle_gbs_readcurjointpose(req):
array = Array() array = Array()
array.n = 3 array.n = 3
array.array = [2,1,5] array.array = [2,5,1]
prettyprint(array) prettyprint(array)
comm.sendArray(array) comm.sendArray(array)
ans = GBSReadCurJointPoseResponse() ans = GBSReadCurJointPoseResponse()
...@@ -432,8 +433,9 @@ def handle_gbs_unlockspindle(req): ...@@ -432,8 +433,9 @@ def handle_gbs_unlockspindle(req):
def handle_gbs_unlocktool(req): def handle_gbs_unlocktool(req):
array = Array() array = Array()
array.n = 3 array.n = 3
array.array = [2,1,7] array.array = [2,7,1]
prettyprint(array) prettyprint(array)
comm.sendArray(array)
ans = GBSUnlockToolResponse() ans = GBSUnlockToolResponse()
ans.err = comm.receiveFloat() ans.err = comm.receiveFloat()
comm.receiveFloat() comm.receiveFloat()
......
...@@ -224,11 +224,13 @@ def test(): ...@@ -224,11 +224,13 @@ def test():
ans = linmove(gbs_linmove, 2, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 1, 1) ans = linmove(gbs_linmove, 2, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 1, 1)
ans = linmove(gbs_linmove, 0, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 1, 1) ans = linmove(gbs_linmove, 0, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 1, 1)
#ans = ptpmove(gbs_ptpmove, 1, 2, 3, 4, 5, 6, 10, 6) ans = ptpmove(gbs_ptpmove, 2000, 0, 0, 0, 5, 0, 1, 1)
ans = ptpmove(gbs_ptpmove, 1000, 0, 0, 0, 5, 0, 1, 1)
#readlibrary(array) #readlibrary(array)
#ans = readcurrentrobotpos(gbs_jointpose) ans = readcurrentrobotpos(gbs_jointpose)
#ans = locktool(gbs_locktool) print 'Current robot pose ['+str(ans.p.j1)+','+str(ans.p.j2)+','+str(ans.p.j3)+','+str(ans.p.j4)+','+str(ans.p.j5)+','+str(ans.p.j6)+']'
#ans = unlocktool(gbs_unlocktool) ans = locktool(gbs_locktool)
ans = unlocktool(gbs_unlocktool)
#ans = measuremove(gbs_measuremove, 111, 10, 11, 12, 35, 0, 0, 10, 6) #ans = measuremove(gbs_measuremove, 111, 10, 11, 12, 35, 0, 0, 10, 6)
#ans = naillinmove(gbs_naillinmove, 1, 1, 3, 3, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12) #ans = naillinmove(gbs_naillinmove, 1, 1, 3, 3, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12)
#ans = circmove(gbs_circmove, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, 0, 0, 10, 5) #ans = circmove(gbs_circmove, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, 0, 0, 10, 5)
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment