I
n
t
e
r
n
at
ion
al
Jou
r
n
a
l
of
Robot
ics
an
d
Au
t
om
at
ion
(
I
JRA
)
Vo
l
.
9
,
N
o
.
2
,
J
un
e
2020
,
pp.
7
3
~
83
I
S
S
N:
2089
-
4856
,
DO
I
:
10
.
11591/
i
j
r
a
.
v
9
i
2
.
pp
73
-
83
73
Jou
r
n
al
h
o
m
e
page
:
ht
tp:
//
ij
r
a.
iaes
c
or
e
.
c
om
F
u
z
z
y
l
ogi
c
c
on
t
r
ol
le
r
d
e
si
gn
f
or
P
U
M
A
560 r
ob
ot
m
an
i
p
u
l
at
or
Abd
e
l
-
A
z
im
S
.
Abd
e
l
-
S
al
am
,
I
b
r
ah
im
N.
Jl
e
t
a
D
e
p
art
men
t
o
f
E
l
ec
t
ri
c
a
l
an
d
C
o
m
p
u
t
e
r
E
n
g
i
n
ee
ri
n
g
,
T
h
e
L
i
b
y
a
n
A
c
a
d
emy
,
L
i
b
y
a
Ar
t
ic
l
e
I
n
f
o
AB
S
T
RA
CT
A
r
ti
c
le
h
is
tor
y
:
R
e
c
e
i
ve
d
O
ct
1
,
201
9
R
e
vi
s
e
d
J
a
n
13
,
20
2
0
A
c
c
e
pt
e
d
F
eb
11
,
20
20
T
h
e
d
y
n
ami
c
mo
d
el
o
f
t
h
e
r
o
b
o
t
m
a
n
i
p
u
l
at
o
r
c
o
n
t
ai
n
f
ro
m
e
q
u
at
i
o
n
s
,
t
h
e
s
e
e
q
u
at
i
o
n
s
ar
e
n
o
n
l
i
n
e
ar
an
d
c
o
n
t
ai
n
ed
fr
o
m
v
ar
i
at
i
o
n
s
p
aram
e
t
e
rs
d
u
e
t
o
v
ari
at
i
o
n
s
i
n
l
o
a
d
,
fri
c
t
i
o
n
,
a
n
d
d
i
s
t
u
rb
an
ce.
T
h
e
co
n
v
e
n
t
i
o
n
a
l
c
o
m
p
u
t
e
d
t
o
rq
u
e
(PD
an
d
PID
)
co
n
t
ro
l
l
e
rs
are
n
o
t
h
i
g
h
l
y
s
u
i
t
ab
l
e
fo
r
n
o
n
l
i
n
e
ar,
c
o
m
p
l
e
x
,
t
i
m
e
-
v
ari
a
n
t
s
y
s
t
em
s
w
i
t
h
d
el
a
y
.
I
n
t
h
i
s
p
ap
e
r,
t
h
e
f
u
zz
y
l
o
g
i
c
c
o
n
t
ro
l
l
e
rs
(FL
C)
h
as
b
ee
n
u
s
ed
b
ec
a
u
s
e
i
t
i
s
e
ffi
ci
e
n
t
t
o
o
l
s
fo
r
c
o
n
t
ro
l
o
f
n
o
n
l
i
n
e
ar
an
d
u
n
c
e
rt
ai
n
p
arame
t
e
rs
s
y
s
t
em
s
.
T
h
i
s
p
ap
e
r
ai
m
s
t
o
d
e
s
i
g
n
a
f
u
zz
y
l
o
g
i
c
c
o
n
t
ro
l
l
e
r
fo
r
p
o
s
i
t
i
o
n
c
o
n
t
ro
l
o
f
a
P
U
MA
5
6
0
ro
b
o
t
m
an
i
p
u
l
at
o
r.
Bas
e
d
o
n
s
i
m
u
l
at
i
o
n
r
e
s
u
l
t
s
w
e
co
n
cl
u
d
e
t
h
at
t
h
e
p
e
rf
o
rm
a
n
ce
o
f
t
h
e
f
u
zz
y
l
o
g
i
c
c
o
n
t
ro
l
l
e
r
i
n
t
e
r
m
o
f
p
o
s
i
t
i
o
n
t
ra
c
k
i
n
g
e
rro
r
i
n
cas
e
o
f
d
i
s
t
u
rb
an
ce
o
r
l
o
ad
i
s
b
e
t
t
e
r
t
h
a
n
t
h
e
co
n
v
e
n
t
i
o
n
a
l
co
m
p
u
t
ed
t
o
rq
u
e
(P
D
-
CT
C
an
d
PID
-
CT
C)
co
n
t
ro
l
l
e
rs
.
K
e
y
w
o
r
d
s
:
F
uz
z
y
l
o
g
i
c
c
o
n
t
r
o
l
l
e
r
(
F
L
C
)
P
I
D
-
c
o
m
put
e
d
to
r
que
c
o
n
t
r
o
l
l
e
r
(
P
I
D
-
C
T
C
)
P
UM
A
560
r
o
b
ot
m
a
ni
pu
l
a
to
r
Th
i
s
i
s
a
n
o
p
en
a
c
ces
s
a
r
t
i
c
l
e
u
n
d
e
r
t
h
e
CC
B
Y
-
SA
l
i
cen
s
e.
C
or
r
e
s
pon
din
g
A
u
th
or
:
Ab
de
l
-
A
z
im
S
.
Ab
d
e
l
-
S
a
l
a
m
De
pa
r
t
m
e
n
t
o
f
E
l
e
c
t
r
i
c
a
l
a
n
d
C
o
m
put
e
r
E
n
g
i
n
e
e
r
i
ng,
T
h
e
L
i
b
y
a
n
Ac
a
de
my
,
T
r
i
po
l
i
,
L
i
b
y
a
.
E
-
m
a
il
:
a
b
dua
l
a
de
m
.
s
a
a
i
d
@
a
c
a
de
my
.
e
du.
l
y
1.
I
NT
RODU
C
T
I
ON
T
h
e
m
o
t
i
o
n
o
f
a
m
e
c
ha
ni
c
a
l
m
a
ni
pu
l
a
t
o
r
de
s
c
r
i
b
e
d
us
i
ng
d
y
na
mi
c
e
qua
t
i
o
ns
,
t
h
e
s
e
e
qua
t
i
o
n
s
a
r
e
hi
g
hly
n
o
n
-
li
ne
a
r
a
n
d
c
o
m
p
l
e
x
.
T
h
e
r
e
f
o
r
e
,
i
t
i
s
ve
r
y
d
if
f
i
c
u
l
t
to
i
m
p
l
e
m
e
n
t
r
e
a
l
-
t
i
m
e
c
o
n
t
r
o
l
b
a
s
e
d
o
n
a
de
t
a
i
l
e
d
d
y
na
mi
c
m
o
de
l
o
f
a
r
o
b
ot
,
t
h
e
n
o
n
-
l
i
n
e
a
r
i
t
y
o
f
r
o
b
o
t
m
a
ni
pu
l
a
t
o
r
a
s
c
e
n
t
r
i
f
uga
l
,
f
r
i
c
t
i
o
n
a
nd
gr
a
vi
t
y
m
a
ke
a
d
i
s
t
ur
b
a
n
c
e
i
n
t
hi
s
c
a
s
e
t
h
e
c
o
nv
e
n
t
i
o
n
a
l
c
o
n
t
r
o
l
i
s
n
ot
e
f
f
e
c
t
e
d
to
c
o
n
tr
o
l
o
f
t
he
r
o
b
ot
m
a
ni
pu
l
a
t
o
r
.
A
b
e
tt
e
r
s
o
l
ut
i
o
n
to
t
h
e
c
o
m
p
l
e
x
c
o
n
t
r
o
l
pr
o
bl
e
m
mi
g
h
t
r
e
s
u
l
t
i
f
h
u
m
a
n
i
n
t
e
ll
i
ge
nc
e
a
n
d
j
udg
m
e
n
t
r
e
pl
a
c
e
t
h
e
de
s
i
g
n
a
ppr
o
a
c
h
o
f
f
i
nd
i
ng
a
n
a
ppr
o
xi
m
a
t
i
o
n
to
t
h
e
t
r
ue
r
o
b
ot
s
y
s
t
e
m
[
1]
.
T
h
e
f
u
z
z
y
l
o
g
i
c
c
o
n
t
r
o
l
l
e
r
c
a
n
b
e
u
s
e
d
t
o
o
v
e
r
c
o
m
e
t
hi
s
pr
o
bl
e
m
.
T
hi
s
i
s
be
c
a
us
e
t
h
e
pe
r
f
o
r
m
a
n
c
e
o
f
f
uz
z
y
l
o
g
i
c
c
o
n
t
r
o
l
l
e
r
s
a
r
e
b
e
t
t
e
r
t
h
a
n
a
c
o
nv
e
n
t
i
o
n
a
l
c
o
n
t
r
o
l
l
e
r
b
e
c
a
us
e
a
f
uz
z
y
l
o
g
i
c
a
l
g
o
r
i
t
hm
s
do
n
ot
r
e
qui
r
e
a
d
e
t
a
i
l
e
d
m
a
t
h
e
m
a
t
i
c
a
l
de
s
c
r
i
pt
i
o
n
o
f
t
h
e
d
y
na
mi
c
m
o
de
l
t
o
b
e
c
o
n
t
r
o
l
l
e
d
,
a
n
d
t
h
e
f
uz
z
y
l
o
g
i
c
c
o
n
t
r
o
l
l
e
r
c
o
n
s
i
de
r
a
s
i
n
t
e
ll
i
ge
n
t
c
o
n
t
r
o
l
l
e
r
e
s
pe
c
i
a
ll
y
i
n
t
h
e
n
o
n
-
l
i
ne
a
r
s
y
s
t
e
m
.
T
he
r
e
f
o
r
e
,
t
h
e
i
m
p
l
e
m
e
n
t
a
t
i
o
n
o
f
t
h
e
f
u
z
z
y
l
o
g
i
c
c
o
n
t
r
o
l
l
e
r
s
h
o
u
l
d
b
e
c
o
m
put
a
t
i
o
n
a
l
l
e
s
s
de
m
a
n
d
i
ng
a
n
d
t
h
e
p
o
s
i
t
i
o
n
c
o
n
t
r
o
l
o
f
a
P
u
m
a
560
r
o
b
o
t
m
a
ni
pu
l
a
t
or
c
a
n
b
e
a
c
hi
e
ve
d
by
us
i
ng
a
f
uz
z
y
l
o
g
i
c
c
o
n
t
r
o
l
l
e
r
.
C
om
put
e
d
to
r
que
c
o
n
tr
o
l
(
C
T
C
)
i
s
e
f
f
e
c
t
i
v
e
w
i
t
h
a
n
o
nli
ne
a
r
s
y
s
t
e
m
a
n
d
us
e
s
i
n
r
o
b
o
t
m
a
ni
pu
l
a
t
o
r
f
o
r
c
o
n
tr
o
l
li
ng,
i
t
i
s
a
pp
lyi
ng
f
e
e
d
ba
c
k
l
i
ne
a
r
i
z
a
t
i
o
n
t
o
n
o
nl
i
ne
a
r
s
y
s
t
e
m
f
o
r
c
o
m
put
e
s
t
h
e
to
r
que
n
e
e
ds
f
o
r
t
h
e
a
r
m
,
i
t
w
or
ks
we
l
l
w
h
e
n
a
l
l
d
y
na
mi
c
a
n
d
p
hy
s
i
c
a
l
p
a
r
a
m
e
t
e
r
s
a
r
e
kn
o
wn
b
ut
i
f
t
h
e
r
o
b
ot
h
a
s
a
d
y
na
mi
c
pa
r
a
m
e
t
e
r
v
a
r
i
a
t
i
o
n
,
t
h
e
n
t
h
e
pe
r
f
o
r
m
a
n
c
e
o
f
t
h
e
c
o
n
tr
o
l
l
e
r
w
il
l
n
o
t
b
e
a
c
c
e
pt
a
bl
e
[
2]
.
Z
a
de
h
i
n
1965
pr
e
s
e
n
t
e
d
hi
s
pa
pe
r
o
n
f
uz
z
y
s
e
t
s
a
nd
f
u
z
z
y
l
o
g
i
c
he
wa
s
t
h
e
f
i
r
s
t
o
n
e
us
e
t
h
e
t
h
e
o
r
y
o
f
t
h
e
f
u
z
z
y
s
e
t
a
n
d
f
uz
z
y
l
o
g
i
c
[
3]
,
a
n
d
i
n
t
r
o
duc
e
t
h
e
c
o
n
c
e
pt
o
f
l
i
n
gu
i
s
t
i
c
va
r
i
a
bl
e
i
n
1973,
Z
a
de
h
s
h
o
we
d
t
h
a
t
f
uz
z
y
l
o
g
i
c
i
n
c
o
n
t
r
a
s
t
c
l
a
s
s
i
c
a
l
l
o
g
i
c
m
a
y
t
a
ke
v
a
l
ue
s
b
e
t
we
e
n
f
a
l
s
e
a
n
d
t
r
ue
,
i
n
c
l
a
s
s
i
c
a
l
s
e
t
t
h
e
o
r
y
de
f
i
n
i
t
i
o
n
o
f
m
e
m
b
e
r
s
hi
p
f
u
nc
t
i
o
n
do
e
s
n
o
t
m
a
t
t
e
r
,
b
ut
t
h
e
n
u
m
be
r
b
e
l
o
n
g
s
t
o
or
do
e
s
n
o
t
b
e
l
o
n
g
t
o
th
e
s
e
t
,
y
e
s
o
r
n
o
a
n
d
a
l
s
o
t
h
e
z
e
r
o
or
o
n
e
t
a
ke
s
o
n
t
h
e
v
a
l
ue
,
t
hi
s
a
ppr
o
a
c
h
i
s
n
o
t
a
ppr
o
pr
i
a
t
e
i
n
m
a
ny
li
f
e
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
S
N
:
2089
-
4856
I
n
t
J
R
o
b
&
A
ut
o
m
,
Vo
l
.
9
,
N
o
.
2
,
J
un
e
2020
:
7
3
–
83
74
a
pp
l
i
c
a
t
i
o
ns
li
ke
t
he
s
e
t
a
ge
d
o
r
t
h
e
s
e
t
o
f
t
e
m
p
e
r
a
t
ur
e
,
b
ut
t
h
e
e
l
e
m
e
n
t
i
n
t
h
e
f
uz
z
y
s
e
t
c
o
n
t
a
i
n
s
m
o
vi
ng
v
a
l
ue
s
b
e
t
we
e
n
0
a
n
d
1,
m
e
a
ni
n
g
t
ha
t
e
l
e
m
e
n
t
s
o
f
t
h
e
s
e
s
e
t
s
n
o
t
o
nl
y
r
e
pr
e
s
e
n
t
t
r
ue
o
r
f
a
l
s
e
v
a
l
ue
but
a
l
s
o
r
e
pr
e
s
e
n
t
t
h
e
de
gr
e
e
o
f
t
r
u
t
h
o
r
t
h
e
de
gr
e
e
o
f
f
a
l
s
i
t
y
o
f
e
a
c
h
i
nput
[
4
]
.
T
h
e
P
UM
A
560
r
o
b
ot
i
s
a
n
a
bb
r
e
vi
a
t
i
o
n
f
o
r
(
P
r
o
g
r
a
m
m
a
bl
e
U
ni
ve
r
s
a
l
M
a
ni
pu
l
a
t
o
r
f
o
r
A
s
s
e
m
b
ly
)
r
e
l
e
a
s
e
d
i
n
1978
wa
s
t
h
e
f
i
r
s
t
m
o
de
r
n
i
ndu
s
t
r
i
a
l
r
o
b
ot
a
n
d
b
e
c
a
m
e
de
s
p
e
r
a
t
e
l
y
po
pu
l
a
r
,
i
t
i
s
a
n
i
nd
us
t
r
i
a
l
r
o
b
ot,
t
hi
s
r
o
b
o
t
h
a
s
6
de
gr
e
e
s
o
f
f
r
e
e
do
m
w
i
t
h
6
r
ot
a
t
i
o
n
a
l
j
o
i
n
t
s
,
t
h
e
P
u
m
a
560
wa
s
u
s
e
d
f
o
r
r
e
s
e
a
r
c
h
in
t
h
e
1980s
a
n
d
i
t
wa
s
a
v
e
r
y
c
o
m
m
o
n
l
a
b
o
r
a
to
r
y
r
o
b
ot,
we
us
e
d
a
l
ot
i
n
r
e
s
e
a
r
c
h
be
c
a
us
e
i
t
h
a
s
b
e
e
n
we
l
l
s
t
udi
e
d
a
n
d
i
t
i
s
pa
r
a
m
e
t
e
r
s
we
l
l
-
k
n
o
wn
h
a
v
e
b
e
e
n
de
s
c
r
i
b
e
d
a
s
"
w
hi
t
e
mi
c
e
"
i
n
r
e
s
e
a
r
c
h
r
o
b
ot
s
[
5]
.
2.
T
HE
P
UM
A
560
RO
B
OT
M
ODE
L
T
h
e
s
pe
c
i
f
i
c
a
t
i
o
n
o
f
t
h
e
P
u
m
a
560
r
o
b
ot
m
a
ni
p
u
l
a
t
o
r
us
e
d
i
n
t
hi
s
p
a
pe
r
i
s
f
r
o
m
t
h
e
pa
pe
r
o
f
A
r
m
s
t
r
o
n
g,
K
h
a
t
i
b
,
a
n
d
B
ur
d
i
c
k
[
6]
.
I
n
t
hi
s
pa
p
e
r
,
we
us
e
d
t
h
e
R
o
b
ot
i
c
s
T
oo
l
b
o
x
f
o
r
M
A
T
L
A
B
c
r
e
a
t
e
d
by
P
e
t
e
r
C
o
r
ke
i
n
A
us
t
r
a
l
i
a
t
o
s
i
m
u
l
a
t
i
o
n
a
n
d
im
p
l
e
m
e
n
t
a
t
i
o
n
o
f
t
h
e
pr
o
p
o
s
e
d
c
o
n
t
r
o
l
l
e
r
s
f
o
r
c
o
nt
r
o
l
in
t
h
e
po
s
i
t
i
o
n
o
f
t
h
e
P
u
m
a
560
r
o
b
ot
m
a
ni
pu
l
a
t
or
[
7
]
.
T
o
de
f
i
n
e
d
t
he
m
o
d
i
f
i
e
d
D
-
H
pa
r
a
m
e
t
e
r
s
we
us
e
d
t
hi
s
c
o
m
m
a
n
d
P
u
m
a
560
a
k
b
a
n
d
de
s
c
r
i
pt
i
o
n
c
r
e
a
t
e
s
t
h
e
r
o
b
ot
o
b
j
e
c
t
us
e
d
t
hi
s
c
o
m
m
a
n
d
P
560
m
,
w
hi
c
h
de
s
c
r
i
be
s
t
h
e
k
i
ne
m
a
t
i
c
a
n
d
d
y
na
mi
c
c
h
a
r
a
c
t
e
r
i
s
t
i
c
s
o
f
a
n
a
nim
a
t
i
o
n
P
u
m
a
560
r
o
bot
m
a
ni
pu
l
a
t
o
r
s
[
8
]
.
I
n
t
h
i
s
pa
pe
r
,
t
h
e
m
o
d
i
f
i
e
d
D
-
H
pa
r
a
m
e
t
e
r
s
h
a
v
e
b
e
e
n
us
e
d.
C
r
a
i
g
[
9]
wa
s
t
h
e
f
i
r
s
t
o
n
e
us
e
d
t
h
e
m
o
d
i
f
i
e
d
D
-
H
pa
r
a
m
e
t
e
r
s
i
n
1986.
T
h
e
s
p
e
c
i
f
i
c
a
t
i
o
ns
o
f
k
i
ne
m
a
t
i
c
s
t
a
k
i
n
g
f
r
o
m
t
h
e
pa
pe
r
o
f
A
r
m
s
t
r
o
n
g,
K
ha
t
i
b
,
a
n
d
B
ur
d
i
c
k.
I
n
t
h
e
r
o
b
ot
i
c
s
too
l
b
o
x
,
t
h
e
a
c
t
ua
to
r
s
h
a
v
e
b
e
e
n
i
nc
l
ude
d
i
n
t
he
d
y
na
mi
c
s
m
o
d
e
l
o
f
a
P
u
m
a
560
r
o
b
ot
m
a
ni
p
u
l
a
t
o
r
.
F
i
gur
e
1
s
h
o
w
s
t
h
e
P
u
m
a
560
r
o
b
ot
m
a
ni
pu
l
a
t
or
a
c
c
o
r
di
n
g
to
s
pe
c
i
f
i
c
a
t
i
o
n
o
f
A
r
m
s
t
r
o
n
g,
Kha
ti
b
a
nd
B
ur
d
i
c
k
a
n
d
us
e
d
t
h
e
m
o
d
i
f
i
e
d
D
-
H
pa
r
a
m
e
t
e
r
.
F
i
gur
e
1.
T
h
e
pu
m
a
560
-
AK
B
3.
CONT
ROL
L
E
R
DE
S
I
GN
3.
1.
P
D
Com
p
u
t
e
d
t
or
q
u
e
c
on
t
r
o
l
le
r
T
h
e
m
o
t
i
o
n
o
f
t
h
e
r
o
b
ot
de
s
c
r
i
b
e
by
(
r
i
g
i
d
b
o
d
y
d
yn
a
mi
c
s
)
:
τ
)
)+
G(
q)
(
1)
w
h
e
r
e
;
M
(
q
)
I
n
e
r
t
i
a
m
a
t
r
i
x
V(
)
C
e
n
t
r
i
f
uga
l
a
n
d
C
o
r
i
o
l
i
s
t
e
r
m
s
)
F
r
i
c
t
i
o
n
t
e
r
m
G(
q)
Gr
a
vi
t
y
t
e
r
m
s
On
e
wa
y
to
s
e
l
e
c
t
t
h
e
c
o
n
t
r
o
l
s
i
g
na
l
u(
t
)
i
s
a
s
t
h
e
pr
o
p
or
t
i
o
n
a
l
p
l
us
de
r
i
v
a
t
i
v
e
(
P
D)
f
e
e
d
b
a
c
k,
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
ut
o
m
I
S
S
N:
2089
-
4856
F
uz
z
y
logi
c
c
ontr
oll
e
r
de
s
ign
f
or
P
UM
A
560
r
obot
manipulat
or
(
A
bde
l
-
A
z
im
S
.
A
bde
l
-
Salam
)
75
u
kvė
k
pe
(
2)
T
h
e
n
t
h
e
f
u
ll
i
nput
f
o
r
a
P
um
a
560
r
o
b
ot
a
r
m
(
F
i
gu
r
e
2)
b
e
c
o
m
e
s
,
τ
k
v
ė
k
)
(
3)
)
)+
G(
q)
(
4)
T
hi
s
c
o
n
t
r
o
l
l
e
r
i
s
s
h
o
wn
i
n
F
i
gur
e
3
w
i
t
h
k
i
=
0
.
F
i
gur
e
2.
T
h
e
pum
a
560
r
o
b
ot
A
r
m
w
i
t
h
P
D
-
C
T
C
F
i
gur
e
3.
T
h
e
P
D
c
o
m
put
e
d
to
r
que
c
o
n
tr
o
l
l
e
r
[
7]
I
n
t
hi
s
s
im
u
l
a
t
i
o
n
we
us
e
d
s
o
m
e
bl
o
c
ks
f
r
o
m
P
e
t
e
r
C
o
r
ke
,
R
o
b
ot
i
c
s
T
oo
l
b
o
x
,
a
s
f
o
l
l
o
w
i
n
g:
Jt
r
aj
:
t
h
e
pur
p
o
s
e
o
f
t
hi
s
bl
o
c
k
i
s
to
c
o
m
put
e
a
j
o
i
n
t
s
pa
c
e
t
r
a
j
e
c
t
o
r
y
be
t
we
e
n
t
wo
j
o
i
n
t
c
oo
r
di
na
t
e
po
s
e
s
.
T
h
e
c
o
m
m
a
n
d
i
ns
i
de
t
h
e
bl
o
c
k
i
s
:
q
qd
qdd
]
=
j
t
r
a
j
(
q0,
q1,
n
)
(
5)
RN
E
:
t
h
e
pur
po
s
e
o
f
t
hi
s
bl
o
c
k
i
s
to
c
o
m
put
e
i
nve
r
s
e
d
y
na
mi
c
s
by
R
e
c
ur
s
i
ve
Ne
wto
n
-
E
ul
e
r
m
e
t
h
o
d.
T
h
e
c
o
m
m
a
n
d
i
ns
i
de
t
h
e
bl
o
c
k
i
s
:
t
a
u=
r
n
e
(
r
o
b
ot
,
q
,
qd,
qdd
)
(
6)
P
u
m
a
560
-
AK
B
:
t
h
e
pur
p
o
s
e
o
f
t
hi
s
bl
o
c
k
i
s
to
c
r
e
a
t
e
a
P
um
a
560
r
o
b
ot
.
T
h
e
c
o
n
v
e
n
t
i
o
n
o
f
t
his
r
o
b
ot
t
a
ke
s
f
r
o
m
A
r
m
s
t
r
o
n
g,
K
h
a
t
i
b
,
a
n
d
B
ur
d
i
c
k.
T
hi
s
r
o
b
ot
us
e
d
C
r
a
i
g
'
s
m
o
d
i
f
i
e
d
D
-
H
pa
r
a
m
e
t
e
r
.
Robot
p
l
ot
:
t
h
e
pur
p
o
s
e
o
f
t
hi
s
bl
o
c
k
i
s
gr
a
phi
c
a
l
r
o
b
ot
a
ni
m
a
t
i
o
n
.
T
h
e
c
o
m
m
a
n
d
i
ns
i
d
e
t
h
e
bl
o
c
k
i
s
:
p
l
o
t(
r
o
b
ot,
q)
(
7)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
S
N
:
2089
-
4856
I
n
t
J
R
o
b
&
A
ut
o
m
,
Vo
l
.
9
,
N
o
.
2
,
J
un
e
2020
:
7
3
–
83
76
3.
2.
P
I
D
Com
p
u
t
e
d
T
o
r
q
u
e
Cont
r
o
ll
e
r
W
e
n
o
t
e
d
P
D
c
o
m
put
e
d
tor
que
c
o
n
t
r
o
l
i
s
v
e
r
y
e
f
f
e
c
t
i
v
e
w
h
e
n
a
ll
pa
r
a
m
e
t
e
r
s
f
o
r
t
h
e
a
r
m
a
r
e
kn
o
w
n
a
n
d
n
o
di
s
t
ur
b
a
nc
e
τd,
f
r
o
m
c
l
a
s
s
i
c
a
l
c
o
n
t
r
o
l
t
h
e
o
r
y
i
f
d
i
s
t
ur
b
a
nc
e
s
a
r
e
c
o
n
s
t
a
n
t
t
h
e
P
D
c
o
n
tr
o
l
g
i
ve
s
a
n
o
n
z
e
r
o
s
t
e
a
dy
-
s
t
a
t
e
e
r
r
or
,
f
o
r
m
a
k
i
ng
t
h
e
s
y
s
t
e
m
t
y
pe
(
I
)
we
i
n
c
l
ud
i
ng
a
n
i
n
t
e
gr
a
to
r
i
n
t
h
e
f
e
e
d
f
o
r
wa
r
d
l
o
o
p
us
i
n
g
P
I
D
c
o
m
put
e
d
to
r
que
c
o
n
tr
o
l
l
e
r
[
10]
a
s
i
n
F
i
gur
e
4
.
F
i
gur
e
5
s
h
o
ws
T
h
e
P
u
m
a
560
r
o
b
ot
a
r
m
w
i
t
h
P
I
D
-
C
T
C
.
ε˙
e
(
8)
u
kvė
k
pe
k
iε
(
9)
w
h
e
r
e
t
h
e
a
r
m
s
c
o
n
t
r
o
l
b
e
c
o
m
e
s
a
s
τ
k
v
ė
k
k
iε
(
10)
F
i
gur
e
4.
T
h
e
P
I
D
c
o
m
put
e
d
to
r
que
c
o
n
t
r
o
l
l
e
r
[
6]
F
i
gur
e
5.
T
h
e
P
um
a
560
r
o
b
ot
a
r
m
w
i
t
h
P
I
D
-
C
T
C
3.
3.
F
u
z
z
y
l
ogic
c
on
t
r
oll
e
r
T
h
e
n
e
x
t
s
t
e
ps
s
h
o
w
i
n
g
t
h
e
m
e
t
h
o
d
to
de
s
i
g
n
t
h
e
f
uz
z
y
l
o
g
i
c
c
o
n
t
r
o
l
l
e
r
.
De
f
i
ne
t
h
e
i
n
put
a
n
d
o
u
t
pu
t
to
F
L
C
(
F
i
gur
e
6
)
,
t
h
e
r
e
a
r
e
t
w
o
i
n
put
s
o
f
F
L
C
,
t
h
e
e
r
r
or
e
(
t)
a
n
d
c
h
a
n
ge
o
f
e
r
r
o
r
e
(
t)
a
n
d
o
n
e
o
u
tpu
t
i
s
a
c
o
n
t
r
o
l
s
i
g
n
a
l
u
(
t)
to
t
h
e
p
l
a
n
t
(
F
i
gur
e
s
7
a
n
d
8
)
.
F
uz
z
if
yi
ng
t
h
e
i
n
put
a
n
d
t
h
e
o
u
t
pu
t
v
a
r
i
a
bl
e
s
(
F
i
gu
r
e
9
)
.
I
n
t
h
e
de
s
i
g
n
,
we
c
h
o
s
e
2
i
n
put
w
i
t
h
7
m
e
m
be
r
s
hip
f
u
n
c
t
i
o
n
a
n
d
1
o
u
t
pu
t
wi
t
h
7
m
e
m
be
r
s
hi
p
f
u
n
c
t
i
o
n
(
F
i
gur
e
10
)
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
ut
o
m
I
S
S
N:
2089
-
4856
F
uz
z
y
logi
c
c
ontr
oll
e
r
de
s
ign
f
or
P
UM
A
560
r
obot
manipulat
or
(
A
bde
l
-
A
z
im
S
.
A
bde
l
-
Salam
)
77
I
n
t
h
e
de
s
i
g
n
,
t
h
e
m
e
m
be
r
s
hi
p
f
u
n
c
t
i
o
n
s
e
l
e
c
t
e
d
f
r
o
m
t
h
e
n
e
ga
t
i
v
e
bi
g
N
B
to
t
h
e
p
o
s
i
t
i
v
e
bi
g
PB
.
C
h
o
s
e
t
h
e
i
n
f
e
r
e
n
c
e
m
e
c
h
a
ni
s
m
r
u
l
e
t
o
f
i
nd
t
h
e
r
e
l
a
t
i
o
n
b
e
t
we
e
n
t
h
e
i
nput
a
n
d
o
ut
pu
t,
i
n
t
hi
s
pa
pe
r
we
us
e
d
t
h
e
M
a
m
da
ni
i
nf
e
r
e
nc
e
m
e
c
h
a
ni
s
m
De
f
uz
z
if
yi
ng
t
h
e
o
ut
pu
t
v
a
r
i
a
bl
e
o
f
t
h
e
f
uz
z
y
m
e
c
h
a
ni
s
m
,
de
f
uz
z
if
i
c
a
t
i
o
n
m
e
t
h
o
d
wa
s
us
e
d
i
n
t
hi
s
p
a
pe
r
c
e
n
t
e
r
o
f
gr
a
vi
t
y
(
C
OG
)
.
NB
m
e
a
ns
Ne
ga
t
i
v
e
B
i
g,
N
M
m
e
a
ns
Ne
g
a
t
i
ve
M
e
d
i
u
m
,
NS
m
e
a
n
s
Ne
ga
t
i
v
e
S
m
a
l
l
,
Z
E
m
e
a
n
s
Z
e
r
o
,
P
S
m
e
a
ns
P
o
s
i
t
i
v
e
S
m
a
ll
,
P
M
m
e
a
n
s
P
o
s
i
t
i
ve
M
e
d
i
u
m
a
n
d
P
B
m
e
a
n
s
P
o
s
i
t
i
v
e
B
i
g.
T
h
e
r
u
l
e
ba
s
e
i
s
i
n
T
a
bl
e
1.
τ
τ
f
uz
z
y
(
11)
F
i
gur
e
6
.
T
h
e
pum
a
560
r
o
b
ot
A
r
m
w
i
t
h
F
L
C
F
i
gur
e
7
.
M
e
m
b
e
r
s
hi
p
f
u
n
c
t
i
o
n
o
f
e
(
t)
F
i
gur
e
8
.
M
e
m
b
e
r
s
hi
p
f
u
n
c
t
i
o
n
o
f
e
(
t)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
S
N
:
2089
-
4856
I
n
t
J
R
o
b
&
A
ut
o
m
,
Vo
l
.
9
,
N
o
.
2
,
J
un
e
2020
:
7
3
–
83
78
F
i
gur
e
9
.
T
h
e
f
uz
z
y
l
o
g
i
c
c
o
n
t
r
o
l
l
e
r
s
u
b
s
y
s
t
e
m
F
i
gur
e
10
.
M
e
m
b
e
r
s
hi
p
f
u
n
c
t
i
o
n
o
f
o
u
t
pu
t
T
a
bl
e
1.
R
u
l
e
b
a
s
e
O
ut
put
C
ha
nge
of
E
r
r
o
r
NB
NM
NS
ZE
PS
PM
PB
E
r
r
o
r
NB
NB
NB
NB
NM
NM
NM
NM
NM
NM
NS
NS
NS
NS
NS
NS
NS
NS
ZE
NS
NS
NS
ZE
PS
PS
PS
PS
PS
PS
PS
PS
PS
PS
PS
PM
PM
PM
PM
PM
PB
PB
PB
4.
S
I
M
UL
AT
I
ON
RE
S
UL
T
S
T
o
t
e
s
t
i
n
g
t
h
e
P
u
m
a
560
r
o
b
ot
m
a
ni
pu
lato
r
,
t
h
e
j
o
i
n
t
de
s
i
r
e
d
i
n
put
a
n
g
l
e
s
a
r
e
θ
f
i
na
l
=
[
90˚
,
-
90˚
,
90˚
,
45˚
,
66˚
,
15˚
]
wi
t
h
t
h
e
i
n
i
t
i
a
l
po
s
i
t
i
o
n
o
f
t
h
e
P
u
m
a
560
r
o
b
ot
m
a
ni
pu
lato
r
i
s
t
h
e
z
e
r
o
p
o
s
i
t
i
o
n
θ
i
ni
t
i
a
l
=
[
0˚
,
0˚
,
0˚
,
0
˚
,
0˚
,
0˚
]
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
ut
o
m
I
S
S
N:
2089
-
4856
F
uz
z
y
logi
c
c
ontr
oll
e
r
de
s
ign
f
or
P
UM
A
560
r
obot
manipulat
or
(
A
bde
l
-
A
z
im
S
.
A
bde
l
-
Salam
)
79
4.
1
.
Wit
h
ou
t
d
is
t
u
r
b
an
c
e
T
h
e
pe
r
f
o
r
m
a
n
c
e
o
f
P
D
-
C
T
C
,
P
I
D
-
C
T
C
,
a
n
d
F
L
C
w
i
t
h
o
ut
di
s
t
ur
b
a
n
c
e
a
r
e
goo
d
a
s
s
h
o
w
n
i
n
F
i
g
ur
e
s
11
-
13
,
r
e
s
pe
c
t
i
v
e
ly
.
F
i
gur
e
11.
P
o
s
i
t
i
o
n
t
r
a
c
ki
n
g
c
ur
v
e
o
f
j
o
i
n
t
q5
wi
t
h
P
D
-
C
T
C
F
i
gur
e
12.
P
o
s
i
t
i
o
n
t
r
a
c
ki
n
g
c
ur
v
e
o
f
j
o
i
n
t
q5
wi
t
h
P
I
D
-
C
T
C
F
i
gur
e
13.
P
o
s
i
t
i
o
n
t
r
a
c
ki
n
g
c
ur
v
e
o
f
j
o
i
n
t
q5
wi
t
h
F
L
C
4.
2.
Wit
h
d
is
t
u
r
b
an
c
e
F
o
r
t
e
s
t
i
n
g
t
h
e
P
u
m
a
560
w
i
t
h
d
i
s
t
ur
b
a
n
c
e
,
we
c
h
o
s
e
t
h
e
m
a
xim
u
m
t
o
r
que
o
n
e
a
c
h
j
o
i
n
t
s
f
o
r
c
o
m
put
a
t
i
o
n
t
h
e
d
i
s
t
ur
b
a
n
c
e
to
r
que
t
h
a
t
n
e
e
ds
f
o
r
a
dd
t
o
t
h
e
m
o
de
l
,
we
u
s
e
d
t
h
e
pr
o
gr
a
m
s
h
o
ws
i
n
F
i
gur
e
14.
W
e
t
a
ke
10%
f
r
o
m
t
h
e
m
a
xi
m
u
m
t
o
r
que
to
ge
t
o
n
t
h
e
d
i
s
t
ur
b
a
n
c
e
to
r
que
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
S
N
:
2089
-
4856
I
n
t
J
R
o
b
&
A
ut
o
m
,
Vo
l
.
9
,
N
o
.
2
,
J
un
e
2020
:
7
3
–
83
80
τd=
[
3.
95
-
9
.
87
0.
56
-
0.
095
-
0.
122
0.
031]
.
P
um
a
5
60
r
o
b
ot
us
i
n
g
P
D
-
C
T
C
,
P
I
D
-
C
T
C
,
a
n
d
F
L
C
a
r
e
s
h
o
w
n
i
n
F
i
gur
e
s
15
-
17
r
e
s
pe
c
t
i
v
e
ly
.
F
i
gur
e
14.
H
o
w
to
c
o
m
put
e
t
h
e
to
r
que
s
F
i
gur
e
15.
P
o
s
i
t
i
o
n
t
r
a
c
ki
n
g
c
ur
v
e
o
f
j
o
i
n
t
q5
wi
t
h
P
D
-
C
T
C
F
i
gur
e
16.
P
o
s
i
t
i
o
n
t
r
a
c
ki
n
g
c
ur
v
e
o
f
j
o
i
n
t
q5
wi
t
h
P
I
D
-
C
T
C
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
ut
o
m
I
S
S
N:
2089
-
4856
F
uz
z
y
logi
c
c
ontr
oll
e
r
de
s
ign
f
or
P
UM
A
560
r
obot
manipulat
or
(
A
bde
l
-
A
z
im
S
.
A
bde
l
-
Salam
)
81
F
i
gur
e
17.
P
o
s
i
t
i
o
n
t
r
a
c
ki
n
g
c
ur
v
e
o
f
j
o
i
n
t
q5
wi
t
h
F
L
C
4.
3.
Wit
h
l
oad
F
o
r
t
e
s
t
i
n
g
t
h
e
P
u
m
a
560
w
i
t
h
l
o
a
d,
we
a
dde
d
l
o
a
d
to
t
h
e
m
o
de
l
t
hi
s
l
o
a
d
e
qua
l
6
kg
us
i
n
g
t
hi
s
c
o
m
m
a
n
d
P560m.payload(6,[0, 0, 0.1])
T
o
r
e
m
o
v
e
l
o
a
d
we
us
e
d
t
hi
s
c
o
m
m
a
n
d
P560m.payload(0)
4.
4.
T
h
e
e
r
r
o
r
s
p
os
it
ion
t
r
ac
k
in
g
c
u
r
ve
s
T
h
e
po
s
i
t
i
o
n
s
t
r
a
c
k
i
n
g
c
ur
v
e
o
f
P
u
m
a
r
o
b
o
t
us
i
ng
PD
-
C
T
C
,
P
ID
-
C
T
C
,
a
n
d
F
L
C
a
r
e
s
h
o
wn
in
F
i
gur
e
s
18
-
20.
T
h
e
F
i
gur
e
s
21
-
23
s
h
o
w
t
h
e
e
r
r
o
r
s
s
i
g
na
l
s
o
f
t
h
e
po
s
i
t
i
o
n
t
r
a
c
k
i
n
g
c
ur
v
e
s
o
f
t
h
e
c
o
n
t
r
o
l
l
e
r
s
PD
-
C
T
C
,
P
ID
-
C
T
C
,
a
n
d
F
L
C
w
i
t
h
d
i
s
t
ur
b
a
n
c
e
,
r
e
s
pe
c
t
i
v
e
ly
.
F
i
gur
e
18.
P
o
s
i
t
i
o
n
t
r
a
c
ki
n
g
c
ur
v
e
o
f
j
o
i
n
t
q5
wi
t
h
P
D
-
C
T
C
F
i
gur
e
19.
P
o
s
i
t
i
o
n
t
r
a
c
ki
n
g
c
ur
v
e
o
f
j
o
i
n
t
q5
wi
t
h
P
I
D
-
C
T
C
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
S
N
:
2089
-
4856
I
n
t
J
R
o
b
&
A
ut
o
m
,
Vo
l
.
9
,
N
o
.
2
,
J
un
e
2020
:
7
3
–
83
82
F
i
gur
e
20.
P
o
s
i
t
i
o
n
t
r
a
c
ki
n
g
c
ur
v
e
o
f
j
o
i
n
t
q5
wi
t
h
F
L
C
F
i
gur
e
21.
T
h
e
e
r
r
or
s
t
r
a
c
ki
n
g
c
ur
v
e
s
o
f
P
D
-
C
T
C
w
i
t
h
d
i
s
t
ur
b
a
nc
e
F
i
gur
e
22.
T
h
e
e
r
r
or
s
t
r
a
c
ki
n
g
c
ur
v
e
s
o
f
P
I
D
-
C
T
C
w
i
t
h
d
i
s
t
ur
b
a
nc
e
F
i
gur
e
23.
T
h
e
e
r
r
or
s
t
r
a
c
ki
n
g
c
ur
v
e
s
o
f
F
L
C
w
i
t
h
d
i
s
t
ur
b
a
n
c
e
Evaluation Warning : The document was created with Spire.PDF for Python.