Skip to content
GitLab
Menu
Projects
Groups
Snippets
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Mathias Haage
GBS
Commits
884ce343
Commit
884ce343
authored
Jul 26, 2015
by
Mathias Haage
Browse files
Support more gbs cmnds
parent
d3d67b02
Changes
2
Hide whitespace changes
Inline
Side-by-side
catkin_ws/src/gbs/scripts/synch_gbs_server_rs_v3.py
View file @
884ce343
...
...
@@ -82,7 +82,10 @@ def prettyprint(array):
ac
=
array
.
array
[
10
+
i
]
i
=
i
+
11
print
"ptpmove j1=%f j2=%f j3=%f j4=%f j5=%f j6=%f s=%f ac=%f"
%
(
j1
,
j2
,
j3
,
j4
,
j5
,
j6
,
s
,
ac
)
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
4
):
elif
near
(
c0
,
2
)
and
near
(
c1
,
3
)
and
near
(
c2
,
1
):
i
=
i
+
3
print
"noop"
elif
near
(
c0
,
2
)
and
near
(
c1
,
4
)
and
near
(
c2
,
1
):
i
=
i
+
3
print
"readlibrary"
elif
near
(
c0
,
2
)
and
near
(
c1
,
5
)
and
near
(
c2
,
1
):
...
...
@@ -94,38 +97,38 @@ def prettyprint(array):
elif
near
(
c0
,
2
)
and
near
(
c1
,
7
)
and
near
(
c2
,
1
):
i
=
i
+
3
print
"unlocktool"
elif
near
(
c0
,
1
8
)
and
near
(
c1
,
1
)
and
near
(
c2
,
8
):
id
=
array
.
array
[
3
+
i
]
bstop
=
array
.
array
[
4
+
i
]
s
=
array
.
array
[
5
+
i
]
ac
=
array
.
array
[
6
+
i
]
x1
=
array
.
array
[
7
+
i
]
y1
=
array
.
array
[
8
+
i
]
z1
=
array
.
array
[
9
+
i
]
c1
=
array
.
array
[
10
+
i
]
b1
=
array
.
array
[
1
1
+
i
]
a1
=
array
.
array
[
1
2
+
i
]
x2
=
array
.
array
[
1
3
+
i
]
y2
=
array
.
array
[
1
4
+
i
]
z2
=
array
.
array
[
1
5
+
i
]
c2
=
array
.
array
[
1
6
+
i
]
b2
=
array
.
array
[
1
7
+
i
]
a2
=
array
.
array
[
1
8
+
i
]
i
=
i
+
1
9
elif
near
(
c0
,
1
7
)
and
near
(
c1
,
8
):
id
=
array
.
array
[
2
+
i
]
bstop
=
array
.
array
[
3
+
i
]
s
=
array
.
array
[
4
+
i
]
ac
=
array
.
array
[
5
+
i
]
x1
=
array
.
array
[
6
+
i
]
y1
=
array
.
array
[
7
+
i
]
z1
=
array
.
array
[
8
+
i
]
c1
=
array
.
array
[
9
+
i
]
b1
=
array
.
array
[
1
0
+
i
]
a1
=
array
.
array
[
1
1
+
i
]
x2
=
array
.
array
[
1
2
+
i
]
y2
=
array
.
array
[
1
3
+
i
]
z2
=
array
.
array
[
1
4
+
i
]
c2
=
array
.
array
[
1
5
+
i
]
b2
=
array
.
array
[
1
6
+
i
]
a2
=
array
.
array
[
1
7
+
i
]
i
=
i
+
1
8
print
"naillinmove id=%f bstop=%f s=%f ac=%f x1=%f y1=%f z1=%f c1=%f b1=%f a1=%f x2=%f y2=%f z2=%f c2=%f b2=%f a2=%f"
%
(
id
,
bstop
,
s
,
ac
,
x1
,
y1
,
z1
,
c1
,
b1
,
a1
,
x2
,
y2
,
z2
,
c2
,
b2
,
a2
)
elif
near
(
c0
,
1
1
)
and
near
(
c1
,
1
)
and
near
(
c2
,
8
):
id
=
array
.
array
[
3
+
i
]
x
=
array
.
array
[
4
+
i
]
y
=
array
.
array
[
5
+
i
]
z
=
array
.
array
[
6
+
i
]
c
=
array
.
array
[
7
+
i
]
b
=
array
.
array
[
8
+
i
]
a
=
array
.
array
[
9
+
i
]
s
=
array
.
array
[
10
+
i
]
ac
=
array
.
array
[
1
1
+
i
]
i
=
i
+
1
2
print
"measuremove id=%f
x
=%f
y
=%f
z
=%f
c
=%f
b
=%f
a
=%f s=%f ac=%f"
%
(
id
,
x
,
y
,
z
,
c
,
b
,
a
,
s
,
ac
)
elif
near
(
c0
,
16
)
and
near
(
c1
,
1
)
and
near
(
c2
,
1
0
):
elif
near
(
c0
,
1
0
)
and
near
(
c1
,
9
):
id
=
array
.
array
[
2
+
i
]
j1
=
array
.
array
[
3
+
i
]
j2
=
array
.
array
[
4
+
i
]
j3
=
array
.
array
[
5
+
i
]
j4
=
array
.
array
[
6
+
i
]
j5
=
array
.
array
[
7
+
i
]
j6
=
array
.
array
[
8
+
i
]
s
=
array
.
array
[
9
+
i
]
ac
=
array
.
array
[
1
0
+
i
]
i
=
i
+
1
1
print
"measuremove id=%f
j1
=%f
j2
=%f
j3
=%f
j4
=%f
j5
=%f
j6
=%f s=%f ac=%f"
%
(
id
,
j1
,
j2
,
j3
,
j4
,
j5
,
j6
,
s
,
ac
)
elif
near
(
c0
,
16
)
and
near
(
c1
,
1
0
)
and
near
(
c2
,
1
):
xi
=
array
.
array
[
3
+
i
]
yi
=
array
.
array
[
4
+
i
]
zi
=
array
.
array
[
5
+
i
]
...
...
@@ -142,7 +145,7 @@ def prettyprint(array):
ac
=
array
.
array
[
16
+
i
]
i
=
i
+
17
print
"circmove xi=%f yi=%f zi=%f ci=%f bi=%f ai=%f xf=%f yf=%f zf=%f cf=%f bf=%f af=%f s=%f ac=%f"
%
(
xi
,
yi
,
zi
,
ci
,
bi
,
ai
,
xf
,
yf
,
zf
,
cf
,
bf
,
af
,
s
,
ac
)
elif
near
(
c0
,
35
)
and
near
(
c1
,
1
)
and
near
(
c2
,
1
1
):
elif
near
(
c1
,
1
1
)
and
near
(
c2
,
1
):
i
=
i
+
3
print
"combmove"
while
True
:
...
...
@@ -188,32 +191,32 @@ def prettyprint(array):
print
" ptpmove j1=%f j2=%f j3=%f j4=%f j5=%f j6=%f s=%f ac=%f"
%
(
j1
,
j2
,
j3
,
j4
,
j5
,
j6
,
s
,
ac
)
else
:
break
elif
near
(
c0
,
3
)
and
near
(
c1
,
1
)
and
near
(
c2
,
1
2
):
elif
near
(
c0
,
3
)
and
near
(
c1
,
1
2
)
and
near
(
c2
,
1
):
rpm
=
array
.
array
[
3
+
i
]
i
=
i
+
4
print
"runspindle rpm=%f"
%
(
rpm
)
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
1
3
):
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
3
)
and
near
(
c2
,
1
):
i
=
i
+
3
print
"stopspindle"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
1
4
):
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
4
)
and
near
(
c2
,
1
):
i
=
i
+
3
print
"lock
spindle
"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
1
5
):
print
"lock
endeffector
"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
5
)
and
near
(
c2
,
1
):
i
=
i
+
3
print
"unlock
spindle
"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
1
6
):
print
"unlock
endeffector
"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
6
)
and
near
(
c2
,
1
):
i
=
i
+
3
print
"get
sensorvalues
"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
1
7
):
print
"get
measure
"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
7
)
and
near
(
c2
,
1
):
i
=
i
+
3
print
"
take
panel"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
1
8
):
print
"
lock
panel"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
8
)
and
near
(
c2
,
1
):
i
=
i
+
3
print
"
release
panel"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
1
9
):
print
"
unlock
panel"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
9
)
and
near
(
c2
,
1
):
i
=
i
+
3
print
"switchvacuum"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
20
):
elif
near
(
c0
,
2
)
and
near
(
c1
,
20
)
and
near
(
c2
,
1
):
i
=
i
+
3
print
"shakevacuum"
else
:
...
...
@@ -223,12 +226,13 @@ def prettyprint(array):
def
handle_gbs_circmove
(
req
):
array
=
Array
()
array
.
n
=
17
array
.
array
=
[
16
,
1
,
1
0
,
req
.
pi
.
pos
.
x
,
req
.
pi
.
pos
.
y
,
req
.
pi
.
pos
.
z
,
req
.
pi
.
ori
.
c
,
req
.
pi
.
ori
.
b
,
req
.
pi
.
ori
.
a
,
req
.
pf
.
pos
.
x
,
req
.
pf
.
pos
.
y
,
req
.
pf
.
pos
.
z
,
req
.
pf
.
ori
.
c
,
req
.
pf
.
ori
.
b
,
req
.
pf
.
ori
.
a
,
req
.
s
,
req
.
ac
]
array
.
array
=
[
16
,
1
0
,
1
,
req
.
pi
.
pos
.
x
,
req
.
pi
.
pos
.
y
,
req
.
pi
.
pos
.
z
,
req
.
pi
.
ori
.
c
,
req
.
pi
.
ori
.
b
,
req
.
pi
.
ori
.
a
,
req
.
pf
.
pos
.
x
,
req
.
pf
.
pos
.
y
,
req
.
pf
.
pos
.
z
,
req
.
pf
.
ori
.
c
,
req
.
pf
.
ori
.
b
,
req
.
pf
.
ori
.
a
,
req
.
s
,
req
.
ac
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSCircMoveResponse
()
ans
.
err1
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
#ans.err1 = comm.receiveFloat()
#comm.receiveFloat()
ans
.
err1
=
0
ans
.
err2
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
...
...
@@ -236,14 +240,15 @@ def handle_gbs_circmove(req):
def
handle_gbs_combmove
(
req
):
array
=
Array
()
array
.
n
=
3
+
req
.
n
array
.
array
=
[
35
,
1
,
11
]
+
[
-
1
for
i
in
range
(
req
.
n
+
1
)]
array
.
array
=
[
array
.
n
-
1
,
11
,
1
]
+
[
-
1
for
i
in
range
(
req
.
n
+
1
)]
for
i
in
range
(
req
.
n
):
array
.
array
[
3
+
i
]
=
req
.
array
[
i
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSCombMoveResponse
()
ans
.
err1
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
#ans.err1 = comm.receiveFloat()
#comm.receiveFloat()
ans
.
err1
=
0
ans
.
err2
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
...
...
@@ -265,11 +270,11 @@ def handle_gbs_linmove(req):
def
handle_gbs_lockspindle
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
1
4
]
array
.
array
=
[
2
,
1
4
,
1
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSLockSpindleResponse
()
ans
.
err
=
com
.
receiveFloat
()
ans
.
err
=
com
m
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
...
...
@@ -286,26 +291,28 @@ def handle_gbs_locktool(req):
def
handle_gbs_measuremove
(
req
):
array
=
Array
()
array
.
n
=
1
2
array
.
array
=
[
1
1
,
1
,
8
,
req
.
id
,
req
.
p
.
pos
.
x
,
req
.
p
.
pos
.
y
,
req
.
p
.
pos
.
z
,
req
.
p
.
ori
.
c
,
req
.
p
.
ori
.
b
,
req
.
p
.
ori
.
a
,
req
.
s
,
req
.
ac
]
array
.
n
=
1
1
array
.
array
=
[
1
0
,
9
,
req
.
id
,
req
.
p
.
pos
.
x
,
req
.
p
.
pos
.
y
,
req
.
p
.
pos
.
z
,
req
.
p
.
ori
.
c
,
req
.
p
.
ori
.
b
,
req
.
p
.
ori
.
a
,
req
.
s
,
req
.
ac
]
prettyprint
(
array
)
comm
.
sendArray
()
comm
.
sendArray
(
array
)
ans
=
GBSMeasureMoveResponse
()
ans
.
err1
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
#ans.err1 = comm.receiveFloat()
#comm.receiveFloat()
ans
.
err1
=
0
ans
.
err2
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
def
handle_gbs_naillinmove
(
req
):
array
=
Array
()
array
.
n
=
1
9
array
.
array
=
[
1
8
,
1
,
8
,
req
.
id
,
req
.
bstop
,
req
.
s
,
req
.
ac
,
req
.
p1
.
pos
.
x
,
req
.
p1
.
pos
.
y
,
req
.
p1
.
pos
.
z
,
req
.
p1
.
ori
.
c
,
req
.
p1
.
ori
.
b
,
req
.
p1
.
ori
.
a
,
req
.
p2
.
pos
.
x
,
req
.
p2
.
pos
.
y
,
req
.
p2
.
pos
.
z
,
req
.
p2
.
ori
.
c
,
req
.
p2
.
ori
.
b
,
req
.
p2
.
ori
.
a
]
array
.
n
=
1
8
array
.
array
=
[
1
7
,
8
,
req
.
id
,
req
.
bstop
,
req
.
s
,
req
.
ac
,
req
.
p1
.
pos
.
x
,
req
.
p1
.
pos
.
y
,
req
.
p1
.
pos
.
z
,
req
.
p1
.
ori
.
c
,
req
.
p1
.
ori
.
b
,
req
.
p1
.
ori
.
a
,
req
.
p2
.
pos
.
x
,
req
.
p2
.
pos
.
y
,
req
.
p2
.
pos
.
z
,
req
.
p2
.
ori
.
c
,
req
.
p2
.
ori
.
b
,
req
.
p2
.
ori
.
a
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSNailLinMoveResponse
()
ans
.
err1
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
#ans.err1 = comm.receiveFloat()
#comm.receiveFloat()
ans
.
err1
=
0
ans
.
err2
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
...
...
@@ -344,7 +351,7 @@ def handle_gbs_readcurjointpose(req):
def
handle_gbs_releasepanel
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
1
8
]
array
.
array
=
[
2
,
1
8
,
1
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSReleasePanelResponse
()
...
...
@@ -355,16 +362,18 @@ def handle_gbs_releasepanel(req):
def
handle_gbs_runspindle
(
req
):
array
=
Array
()
array
.
n
=
4
array
.
array
=
[
3
,
1
,
1
2
,
req
.
rpm
]
array
.
array
=
[
3
,
1
2
,
1
,
req
.
rpm
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSRunSpindleResponse
()
err
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
def
handle_gbs_readsensor
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
1
6
]
array
.
array
=
[
2
,
1
6
,
1
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSReadSensorResponse
()
...
...
@@ -378,7 +387,7 @@ def handle_gbs_readsensor(req):
def
handle_gbs_shakevacuum
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
20
]
array
.
array
=
[
2
,
20
,
1
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSShakeVacuumResponse
()
...
...
@@ -389,7 +398,7 @@ def handle_gbs_shakevacuum(req):
def
handle_gbs_stopspindle
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
1
3
]
array
.
array
=
[
2
,
1
3
,
1
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSStopSpindleResponse
()
...
...
@@ -400,7 +409,7 @@ def handle_gbs_stopspindle(req):
def
handle_gbs_switchvacuum
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
1
9
]
array
.
array
=
[
2
,
1
9
,
1
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSSwitchVacuumResponse
()
...
...
@@ -411,7 +420,7 @@ def handle_gbs_switchvacuum(req):
def
handle_gbs_takepanel
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
1
7
]
array
.
array
=
[
2
,
1
7
,
1
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSTakePanelResponse
()
...
...
@@ -422,7 +431,7 @@ def handle_gbs_takepanel(req):
def
handle_gbs_unlockspindle
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
1
5
]
array
.
array
=
[
2
,
1
5
,
1
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSUnlockSpindleResponse
()
...
...
catkin_ws/src/gbs/scripts/test_client_synch_rs_v3.py
View file @
884ce343
...
...
@@ -231,19 +231,19 @@ def test():
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
=
locktool
(
gbs_locktool
)
ans
=
unlocktool
(
gbs_unlocktool
)
#
ans = measuremove(gbs_measuremove,
11
1,
1
0,
11, 12, 35
,
0
, 0, 1
0
,
6
)
#
ans = naillinmove(gbs_naillinmove, 1, 1,
3, 3
, 1,
2
,
3
,
4, 5, 6, 7
,
8
,
9
,
10, 11, 12
)
ans
=
measuremove
(
gbs_measuremove
,
1
,
0
,
0
,
0
,
0
,
5
,
0
,
1
,
1
)
ans
=
naillinmove
(
gbs_naillinmove
,
1
,
1
,
1
,
1
,
0
,
0
,
-
0.6
,
math
.
radians
(
0
),
math
.
radians
(
0
),
math
.
radians
(
-
175
)
,
2
,
0
,
-
0.6
,
math
.
radians
(
0
),
math
.
radians
(
0
),
math
.
radians
(
-
175
)
)
#ans = circmove(gbs_circmove, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, 0, 0, 10, 5)
#ans = combmove(gbs_combmove, (1, 0,0,0,0,0,0,10,5, 10, 100,0,0,35,0,0,15,25,35,1,2,3,10,5, 2, 1,2,3,4,5,6,10,5))
#
ans = runspindle(gbs_runspindle, 5000)
#
ans = stopspindle(gbs_stopspindle)
#
ans = lockspindle(gbs_lockspindle)
#
ans = unlockspindle(gbs_unlockspindle)
ans
=
runspindle
(
gbs_runspindle
,
5000
)
ans
=
stopspindle
(
gbs_stopspindle
)
ans
=
lockspindle
(
gbs_lockspindle
)
ans
=
unlockspindle
(
gbs_unlockspindle
)
#ans = getsensorvalues(gbs_readsensor)
#
ans = takepanel(gbs_takepanel)
#
ans = releasepanel(gbs_releasepanel)
#
ans = switchvacuum(gbs_switchvacuum)
#
ans = shakevacuum(gbs_shakevacuum)
ans
=
takepanel
(
gbs_takepanel
)
ans
=
releasepanel
(
gbs_releasepanel
)
ans
=
switchvacuum
(
gbs_switchvacuum
)
ans
=
shakevacuum
(
gbs_shakevacuum
)
except
rospy
.
ServiceException
,
e
:
print
"Service call failed: %s"
%
e
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment