I
nte
rna
t
io
na
l J
o
urna
l o
f
Ro
bo
t
ics a
nd
Aut
o
m
a
t
io
n
(
I
J
RA
)
Vo
l.
5
,
No
.
1
,
Ma
r
ch
2
0
1
6
,
p
p
.
6
1
~
6
6
I
SS
N:
2089
-
4856
61
J
o
ur
na
l ho
m
ep
a
g
e
:
h
ttp
:
//ia
e
s
jo
u
r
n
a
l.c
o
m/o
n
lin
e/in
d
ex
.
p
h
p
/I
J
RA
No
nlinea
r Dyna
mic Mo
deling
a
nd
O
pti
m
a
l Mo
tion
Ana
ly
sis
o
f
T
w
o
-
Link
Ma
nip
ula
tors
M
.
T
a
leza
deh
*
,
M
.
G
ha
za
l
*
,
M
.
T
a
heri
*
,
M.
N
a
ze
m
i
-
Z
a
de
*
*
D
e
p
a
rt
m
e
n
t
o
f
M
e
c
h
a
n
ics
,
Da
m
a
v
a
n
d
Bra
n
c
h
,
Isla
m
ic
A
z
a
d
Un
iv
e
rsit
y
,
Da
m
a
v
a
n
d
,
Ira
n
Art
icle
I
nfo
AB
ST
RAC
T
A
r
ticle
his
to
r
y:
R
ec
eiv
ed
No
v
9
,
2
0
1
5
R
ev
i
s
ed
J
an
2
6
,
2
0
1
6
A
cc
ep
ted
Feb
1
3
,
2
0
1
6
M
a
n
ip
u
lato
rs
a
re
u
se
d
i
n
v
a
rio
u
s
in
d
u
str
ial
a
p
p
l
ica
ti
o
n
s
t
o
p
e
rf
o
rm
v
a
rian
t
o
p
e
ra
ti
o
n
s
su
c
h
a
s
c
o
n
v
e
y
in
g
p
a
y
lo
a
d
s.
Re
g
a
rd
in
g
to
t
h
e
ir
a
p
p
li
c
a
ti
o
n
s,
d
y
n
a
m
ic
m
o
d
e
li
n
g
a
n
d
m
o
ti
o
n
a
n
a
l
y
sis
o
f
m
a
n
ip
u
lato
rs
a
re
k
n
o
w
n
a
s
im
p
o
rtan
t
a
n
d
a
p
p
e
a
li
n
g
tas
k
s.
In
th
is
w
o
rk
,
n
o
n
l
in
e
a
r
d
y
n
a
m
ics
a
n
d
o
p
ti
m
a
l
m
o
ti
o
n
a
n
a
ly
sis
o
f
t
w
o
-
li
n
k
m
a
n
ip
u
lato
rs
a
re
in
v
e
stig
a
t
e
d
.
T
o
d
y
n
a
m
i
c
m
o
d
e
li
n
g
o
f
th
e
s
y
ste
m
,
th
e
Lag
ra
n
g
e
p
rin
c
ip
le
is
e
m
p
lo
y
e
d
a
n
d
n
o
n
li
n
e
a
r
d
y
n
a
m
ic
e
q
u
a
ti
o
n
s
o
f
t
h
e
m
a
n
ip
u
lat
o
r
a
re
p
re
se
n
ted
in
sta
te
-
sp
a
c
e
f
o
r
m
.
T
h
e
n
,
o
p
ti
m
a
l
m
o
ti
o
n
a
n
a
ly
sis
o
f
th
e
n
o
n
l
in
e
a
r
sy
ste
m
is
d
e
v
e
lo
p
e
d
b
a
se
d
o
n
o
p
ti
m
a
l
c
o
n
tr
o
l
t
h
e
o
ry
.
By
m
e
a
n
s
o
f
o
p
ti
m
a
l
c
o
n
tro
l
th
e
o
ry
,
in
d
ire
c
t
so
lu
t
io
n
o
f
p
ro
b
lem
re
su
lt
s
in
a
t
w
o
-
p
o
i
n
t
b
o
u
n
d
a
ry
v
a
lu
e
p
ro
b
lem
w
h
ich
c
a
n
b
e
so
lv
e
d
n
u
m
e
rica
ll
y
.
F
in
a
ll
y
,
in
o
rd
e
r
to
d
e
m
o
n
stra
te
th
e
p
o
w
e
r
a
n
d
e
ff
icie
n
c
y
o
f
m
e
th
o
d
,
a
n
u
m
b
e
r
o
f
si
m
u
latio
n
s are
p
e
r
f
o
r
m
e
d
f
o
r
a
t
wo
-
li
n
k
m
a
n
ip
u
lato
r
w
h
ich
sh
o
w
a
p
p
li
c
a
b
il
it
y
o
f
p
ro
p
o
se
d
m
e
th
o
d
.
K
ey
w
o
r
d
:
D
y
n
a
m
ic
Ma
n
ip
u
la
to
r
Mo
tio
n
Op
ti
m
al
T
w
o
-
lin
k
Co
p
y
rig
h
t
©
201
6
In
s
t
it
u
te o
f
A
d
v
a
n
c
e
d
E
n
g
i
n
e
e
rin
g
a
n
d
S
c
ien
c
e
.
Al
l
rig
h
ts
re
se
rv
e
d
.
C
o
r
r
e
s
p
o
nd
ing
A
uth
o
r
:
M.
T
ah
er
i,
Dep
ar
t
m
en
t o
f
Me
ch
a
n
ics,
Da
m
av
a
n
d
B
r
an
ch
,
I
s
la
m
ic
A
za
d
U
n
iv
er
s
it
y
,
Da
m
av
an
d
,
I
r
an
.
E
m
ail:
m
o
.
tah
er
y
9
3
@
g
m
ail.
c
o
m
1.
I
NT
RO
D
UCT
I
O
N
Fix
ed
an
d
m
o
b
ile
Ma
n
ip
u
lato
r
s
ar
e
u
s
ed
in
m
a
n
y
ap
p
licati
o
n
s
an
d
p
er
f
o
r
m
s
ev
er
al
tas
k
s
s
u
c
h
as
co
n
v
e
y
in
g
p
a
y
lo
ad
s
,
r
eh
ab
ilit
atio
n
tas
k
s
a
n
d
etc
[1
-
6
]
.
I
n
f
a
ct,
d
u
e
to
th
eir
ad
v
a
n
tag
e
o
f
h
ig
h
s
p
ee
d
,
ac
cu
r
ac
y
an
d
r
ep
ea
tab
ilit
y
,
r
o
b
o
t
m
an
ip
u
lato
r
s
h
a
v
e
b
ec
o
m
e
m
aj
o
r
co
m
p
o
n
en
t
o
f
i
n
d
u
s
tr
ial
ap
p
licat
io
n
s
a
n
d
e
v
en
n
o
w
a
d
a
y
s
th
e
y
b
ec
o
m
e
p
ar
t
o
f
r
o
u
ti
n
e
li
f
e.
R
e
g
ar
d
in
g
to
t
h
eir
v
ast
ap
p
licatio
n
s
,
d
y
n
a
m
ic
m
o
d
elin
g
a
n
d
m
o
tio
n
an
al
y
s
is
o
f
s
u
c
h
s
y
s
te
m
s
h
a
v
e
attr
ac
ted
a
g
r
ea
t
d
ea
l
o
f
i
n
ter
e
s
ts
b
y
m
a
n
y
r
o
b
o
tic
r
esear
ch
er
s
.
I
t
is
w
ell
k
n
o
w
n
th
at
r
o
b
o
t
m
a
n
ip
u
la
to
r
s
ar
e
h
ig
h
l
y
n
o
n
li
n
ea
r
,
d
y
n
a
m
icall
y
co
u
p
led
an
d
ti
m
e
-
v
ar
y
i
n
g
s
y
s
te
m
s
a
n
d
th
ei
r
d
y
n
a
m
ic
an
al
y
s
is
is
a
co
m
p
le
x
an
d
ch
alle
n
g
in
g
is
s
u
e.
L
u
h
[
7
]
s
tu
d
ied
in
d
u
s
tr
ial
m
an
ip
u
l
ato
r
s
an
d
d
ev
elo
p
ed
s
o
m
e
co
n
v
en
t
io
n
al
m
eth
o
d
t
o
co
n
tr
o
l
d
y
n
a
m
ic
m
o
tio
n
o
f
th
e
m
an
ip
u
lato
r
s
.
So
n
g
et
al.
[
8
]
in
v
esti
g
a
ted
d
y
n
a
m
ic
m
o
tio
n
o
f
r
o
b
o
tic
m
a
n
ip
u
lato
r
s
a
n
d
p
r
o
p
o
s
ed
a
co
m
p
u
ted
to
r
q
u
e
co
n
tr
o
ller
to
h
a
n
d
le
r
eq
u
ir
e
m
e
n
t
o
f
p
r
ec
is
e
d
y
n
a
m
ica
l
m
o
d
el
s
o
f
r
o
b
o
tic
m
an
ip
u
lato
r
s
.
P
ilta
n
et
al.
[
9
]
p
r
ese
n
ted
d
y
n
a
m
ic
m
o
tio
n
o
f
r
o
b
o
tic
m
an
ip
u
lato
r
s
an
d
d
ev
elo
p
ed
a
n
o
n
li
n
ea
r
co
n
tr
o
l
s
tr
ateg
y
to
m
o
tio
n
co
n
tr
o
l
o
f
h
i
g
h
l
y
n
o
n
li
n
ea
r
d
y
n
a
m
ic
r
o
b
o
t
m
an
ip
u
lato
r
i
n
p
r
ese
n
ce
o
f
u
n
ce
r
tain
ties
.
R
a
h
i
m
i
e
t
al.
[
1
0
]
s
tu
d
ied
d
y
n
a
m
ic
a
n
al
y
s
is
o
f
el
asti
c
m
a
n
ip
u
lato
r
s
.
T
h
ey
i
n
v
e
s
ti
g
ated
tr
aj
ec
to
r
y
o
p
tim
izatio
n
o
f
s
u
c
h
r
o
b
o
t
u
s
in
g
o
p
ti
m
al
co
n
tr
o
l
th
eo
r
y
.
Mo
r
eo
v
er
,
th
e
y
[
1
1
]
p
r
o
p
o
s
ed
f
in
ite
e
le
m
e
n
t
m
e
th
o
d
to
m
o
d
el
d
y
n
a
m
ic
s
o
f
elas
tic
m
an
ip
u
lato
r
s
.
I
n
th
is
w
o
r
k
,
n
o
n
li
n
ea
r
d
y
n
a
m
ics
an
d
o
p
ti
m
al
m
o
tio
n
p
lan
n
i
n
g
o
f
t
w
o
-
l
in
k
m
a
n
ip
u
lato
r
s
ar
e
in
v
e
s
ti
g
ated
.
T
o
d
y
n
a
m
ic
m
o
d
elin
g
o
f
th
e
s
y
s
te
m
,
th
e
L
ag
r
a
n
g
e
p
r
in
cip
le
is
e
m
p
lo
y
ed
an
d
n
o
n
li
n
ea
r
d
y
n
a
m
ic
eq
u
atio
n
s
o
f
th
e
m
an
ip
u
lato
r
ar
e
p
r
esen
ted
in
s
tate
-
s
p
ac
e
f
o
r
m
.
T
h
en
,
o
p
tim
a
l
m
o
ti
o
n
an
al
y
s
i
s
o
f
th
e
n
o
n
li
n
ea
r
s
y
s
te
m
is
d
ev
e
lo
p
ed
b
ased
o
n
o
p
ti
m
al
co
n
tr
o
l
th
e
o
r
y
.
B
y
m
ea
n
s
o
f
o
p
ti
m
a
l
co
n
tr
o
l
th
eo
r
y
,
i
n
d
ir
ec
t
s
o
lu
tio
n
o
f
r
es
u
lt
s
i
n
a
t
w
o
-
p
o
in
t
b
o
u
n
d
ar
y
v
al
u
e
p
r
o
b
lem
wh
ich
ca
n
b
e
s
o
lv
ed
n
u
m
er
ical
l
y
.
Fi
n
all
y
,
in
o
r
d
er
to
d
em
o
n
s
tr
ate
th
e
p
o
w
er
an
d
ef
f
ic
ien
c
y
o
f
m
e
th
o
d
,
a
n
u
m
b
er
o
f
s
i
m
u
la
tio
n
s
ar
e
p
er
f
o
r
m
ed
f
o
r
a
t
w
o
-
li
n
k
m
an
ip
u
lato
r
w
h
ich
s
h
o
w
ap
p
licab
ilit
y
o
f
p
r
o
p
o
s
ed
m
et
h
o
d
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
9
-
4856
IJ
RA
Vo
l.
5
,
No
.
1
,
Ma
r
ch
2
0
1
6
:
6
1
–
6
6
62
2.
DYNA
M
I
C
M
O
DE
L
O
F
M
ANIPUL
AT
O
R
As it
is
s
ee
n
i
n
F
i
g
u
r
e
1
,
a
t
w
o
-
li
n
k
m
a
n
ip
u
la
to
r
is
p
r
esen
ted
.
T
h
e
m
a
n
ip
u
la
to
r
h
as t
w
o
li
n
k
s
an
d
an
en
d
-
e
f
f
eto
r
Fig
u
r
e
1
.
T
h
e
t
w
o
-
li
n
k
m
a
n
ip
u
lato
r
T
h
e
p
ar
am
eter
s
o
f
th
e
t
w
o
-
l
i
n
k
m
a
n
ip
u
lato
r
ar
e
d
ef
in
ed
a
s
:
1
is
th
e
a
n
g
u
lar
d
is
p
lace
m
e
n
t
o
f
t
h
e
f
ir
s
t
l
in
k
o
f
t
h
e
m
a
n
ip
u
lato
r
,
2
is
th
e
a
n
g
u
lar
d
is
p
lace
m
e
n
t
o
f
th
e
s
ec
o
n
d
lin
k
,
1
is
th
e
a
n
g
u
l
ar
v
elo
cit
y
o
f
th
e
f
ir
s
t
lin
k
,
2
is
t
h
e
a
n
g
u
la
r
v
elo
cit
y
o
f
t
h
e
s
ec
o
n
d
li
n
k
,
1
L
is
th
e
len
g
t
h
o
f
th
e
f
ir
s
t
lin
k
o
f
th
e
m
an
ip
u
lato
r
,
2
L
is
th
e
le
n
g
t
h
o
f
th
e
s
eo
n
d
lin
k
o
f
th
e
m
a
n
ip
u
lato
r
,
1
m
is
th
e
m
as
s
o
f
th
e
f
ir
s
t
lin
k
o
f
t
h
e
m
an
ip
u
lato
r
,
2
m
is
t
h
e
m
ass
o
f
t
h
e
s
ec
o
n
d
li
n
k
o
f
t
h
e
m
an
ip
u
l
ato
r
,
p
m
is
t
h
e
m
a
s
s
o
f
t
h
e
p
a
y
lo
ad
an
d
en
d
-
ef
f
ec
to
r
o
f
th
e
m
a
n
ip
u
lato
r
,
1
I
is
th
e
m
o
m
e
n
t o
f
i
n
er
tia
o
f
th
e
f
ir
s
t lin
k
a
n
d
2
I
is
t
h
e
m
o
m
e
n
t o
f
in
er
tia
o
f
t
h
e
s
ec
o
n
d
lin
k
.
T
o
d
er
iv
e
n
o
n
li
n
ea
r
d
y
n
a
m
ic
eq
u
atio
n
s
o
f
t
h
e
m
an
ip
u
lato
r
,
th
e
k
in
e
tic
en
er
g
y
o
f
t
h
e
m
an
ip
u
lato
r
T
an
d
th
e
p
o
ten
tial e
n
er
g
y
U
is
s
tated
as:
2
2
2
2
2
2
2
2
2
2
2
1
1
2
1
2
1
1
2
1
2
1
2
1
2
1
2
1
f
f
p
c
c
c
c
y
x
m
I
y
x
m
I
y
x
m
T
(
1
)
2
1
2
1
1
2
1
2
1
1
2
1
1
1
s
i
n
s
i
n
s
i
n
2
s
i
n
s
i
n
2
L
L
g
m
L
L
g
m
L
g
m
U
p
(
2
)
w
h
er
e
th
e
v
elo
cities o
f
ea
ch
li
n
k
a
n
d
en
d
-
e
f
f
ec
to
r
ar
e
g
iv
e
n
as:
1
1
1
1
s
i
n
2
L
x
c
(
3
)
1
1
1
1
c
o
s
2
L
y
c
(
4
)
2
1
2
1
2
1
1
1
2
s
i
n
2
s
i
n
L
L
x
c
(
5
)
2
1
2
1
2
1
1
1
2
c
o
s
2
c
o
s
L
L
y
c
(
6
)
2
1
2
1
2
1
1
1
s
i
n
s
i
n
L
L
x
p
(
7
)
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
N
o
n
lin
ea
r
Dyn
a
mic
Mo
d
elin
g
a
n
d
Op
tima
l
Mo
tio
n
A
n
a
lysi
s
o
f Tw
o
-
Lin
k
Ma
n
ip
u
la
to
r
s
(M
.
Ta
leza
d
eh
)
63
2
1
2
1
2
1
1
1
c
o
s
c
o
s
L
L
y
p
(
8
)
As t
h
e
d
y
n
a
m
ic
eq
u
atio
n
s
o
f
t
h
e
r
o
b
o
t a
r
e
d
ev
elo
p
ed
b
y
L
ag
r
an
g
e
p
r
in
c
ip
le,
th
e
L
a
g
r
an
g
ia
n
f
u
n
ctio
n
(
L =
T
–
U
)
is
ca
lcu
lated
an
d
s
u
b
s
titu
ted
i
n
t
h
e
L
a
g
r
an
g
ia
n
eq
u
atio
n
:
i
i
i
Q
q
L
q
L
dt
d
(
9
)
w
h
er
e
i
s
t
h
e
g
e
n
er
alize
d
f
o
r
ce
r
elate
d
to
t
h
e
g
e
n
er
ali
ze
co
o
r
d
in
ate.
No
w
,
u
s
i
n
g
L
a
g
r
an
g
ian
eq
u
atio
n
t
h
e
n
o
n
lin
ea
r
d
y
n
a
m
i
c
eq
u
atio
n
s
o
f
t
h
e
s
y
s
te
m
ca
n
b
e
o
b
tain
ed
in
th
e
co
m
p
ac
t f
o
r
m
as
:
B
q
q
V
q
M
)
,
(
(
10
)
I
n
w
h
ich
n
R
is
to
r
q
u
e
v
ec
to
r
e
x
er
ted
to
th
e
j
o
in
ts
,
n
n
R
q
M
)
(
is
t
h
e
i
n
er
tia
m
atr
ix
,
B
is
co
n
s
ta
n
t
i
n
p
u
t
m
a
tr
ix
,
n
R
q
q
V
)
,
(
is
a
v
ec
to
r
w
h
ich
p
r
esen
t
s
co
r
io
lis
an
d
g
r
av
itat
io
n
al
f
o
r
ce
s
.
T
h
e
ab
o
v
e
m
atr
ices a
r
e
g
iv
e
n
as
:
2
2
2
2
2
1
2
2
1
2
2
2
2
2
2
2
2
2
2
1
2
2
1
2
2
2
2
2
2
2
2
2
2
1
2
2
1
2
2
2
2
1
2
2
2
2
1
2
2
1
1
)
3
(
3
1
c
o
s
c
o
s
2
1
3
1
c
o
s
c
o
s
2
1
3
1
c
o
s
2
c
o
s
)
(
3
1
3
1
L
m
m
L
L
m
L
L
m
L
m
L
m
L
m
L
L
m
L
L
m
L
m
L
m
L
m
L
L
m
L
L
m
L
L
m
L
m
L
m
L
m
M
p
p
p
p
p
p
p
p
p
(
11
)
2
2
1
2
1
2
2
2
1
2
2
1
2
s
i
n
2
2
1
s
i
n
2
2
2
1
)
,
(
L
L
m
m
L
L
m
m
q
q
V
p
p
(
12
)
1
0
0
1
B
(
13
)
Fu
r
t
h
er
m
o
r
e,
th
e
n
o
n
li
n
ea
r
eq
u
atio
n
s
o
f
t
h
e
r
o
b
o
t in
s
tate
-
s
p
ac
e
f
o
r
m
ar
e
g
i
v
en
a
s
:
V
B
M
x
x
X
1
4
3
(
14
)
w
h
er
e
t
h
e
s
tate
v
ec
to
r
is
T
T
x
x
x
x
X
2
1
2
1
4
3
2
1
an
d
th
e
to
r
q
u
e
v
ec
to
r
is
r
elate
d
to
to
r
q
u
es e
x
er
ted
to
th
e
f
ir
s
t a
n
d
s
ec
o
n
d
li
n
k
s
3.
O
P
T
I
M
AL
M
O
T
I
O
N
ANA
L
YS
I
S
T
h
e
o
p
tim
al
co
n
tr
o
l t
h
eo
r
y
i
s
w
id
el
y
u
s
ed
m
a
n
y
r
o
b
o
tic
ap
p
licatio
n
s
[
1
2
-
1
4
]
.
I
n
th
i
s
s
ec
tio
n
,
o
p
ti
m
a
l
m
o
tio
n
o
f
th
e
t
w
o
-
li
n
k
m
a
n
ip
u
lato
r
is
a
n
al
y
ze
d
.
T
o
d
o
th
i
s
,
o
p
ti
m
al
o
n
tr
o
l
t
h
eo
r
y
is
e
m
p
l
o
y
ed
.
T
h
e
d
y
n
a
m
i
c
eq
u
atio
n
s
o
f
m
o
b
ile
r
o
b
o
t
in
s
tate
s
p
ac
e
f
o
r
m
i
s
p
r
esu
m
ed
a
s
co
n
s
tr
ai
n
ts
o
f
o
p
ti
m
al
co
n
tr
o
l
p
r
o
b
lem
an
d
it
i
s
ai
m
ed
to
d
eter
m
i
n
e
o
p
ti
m
al
s
t
ate
v
ec
to
r
X
*
a
n
d
o
p
ti
m
al
co
n
tr
o
l
v
ec
to
r
u
*
w
h
ic
h
f
o
l
lo
w
in
g
o
b
j
ec
tiv
e
f
u
n
c
tio
n
ca
n
b
e
m
i
n
i
m
ized
[
1
1
]
:
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
9
-
4856
IJ
RA
Vo
l.
5
,
No
.
1
,
Ma
r
ch
2
0
1
6
:
6
1
–
6
6
64
dt
t
t
u
t
X
L
u
X
J
f
t
t
0
),
(
),
(
)
,
(
(
15
)
T
h
e
i
n
d
ir
ec
t
s
o
lu
tio
n
o
f
o
p
ti
m
al
co
n
tr
o
l
p
r
o
b
lem
i
s
p
r
esen
ted
w
h
ic
h
b
eg
i
n
s
f
r
o
m
f
o
r
m
in
g
t
h
e
Ha
m
ilto
n
ia
n
f
u
n
ctio
n
X
L
H
T
w
h
er
e
is
d
en
o
ted
as
co
-
s
tate
v
ec
to
r
.
T
h
en
n
ec
e
s
s
ar
y
co
n
d
itio
n
s
f
o
r
o
p
tim
a
l
m
o
tio
n
ar
e
o
b
tain
ed
as th
e
f
o
llo
w
in
g
eq
u
atio
n
w
h
ic
h
is
a
t
w
o
p
o
in
t b
o
u
n
d
ar
y
v
al
u
e
p
r
o
b
lem
:
t
t
t
u
t
X
H
t
X
),
(
),
(
),
(
)
(
*
*
*
*
(
16
)
t
t
t
u
t
X
X
H
t
),
(
),
(
),
(
)
(
*
*
*
*
(
17
)
t
t
t
u
t
X
u
H
),
(
),
(
),
(
0
*
*
*
(
18
)
B
u
t
d
eter
m
i
n
i
n
g
t
h
e
ap
p
r
o
p
r
iate
co
s
t
f
u
n
ctio
n
is
a
n
i
m
p
o
r
t
an
t
tas
k
i
n
o
p
ti
m
al
co
n
tr
o
l
f
o
r
m
u
la
tio
n
an
d
m
u
s
t
b
e
co
n
s
id
er
ed
th
o
r
o
u
g
h
l
y
.
Fo
r
o
p
ti
m
al
m
o
tio
n
p
lan
n
i
n
g
o
f
m
o
b
ile
r
o
b
o
t
th
e
co
s
t
f
u
n
ctio
n
is
ass
u
m
ed
as
a
m
i
n
i
m
u
m
e
n
er
g
y
f
u
n
ctio
n
w
h
ic
h
in
c
lu
d
es
s
p
ee
d
an
d
to
r
q
u
e
o
f
ac
tu
ato
r
s
.
T
h
er
ef
o
r
e,
th
e
co
s
t
f
u
n
ctio
n
ca
n
b
e
r
e
w
r
i
tten
a
s
:
dt
u
X
dt
t
t
u
t
X
L
u
X
J
f
f
t
t
R
W
t
t
0
0
2
2
2
1
2
1
),
(
),
(
)
,
(
(
19
)
w
h
er
e
2
W
X
is
th
e
g
e
n
er
alize
d
s
q
u
a
r
ed
n
o
r
m
o
f
s
tate
v
ec
to
r
w
it
h
r
esp
ec
t
to
s
tate
w
e
ig
h
ti
n
g
m
at
r
ix
W
a
n
d
2
R
u
is
g
e
n
er
alize
d
s
q
u
ar
ed
n
o
r
m
o
f
co
n
tr
o
l
v
ec
to
r
w
it
h
r
e
s
p
ec
t to
co
n
tr
o
l
w
ei
g
h
t
in
g
m
at
r
ix
R
4.
SI
M
UL
AT
I
O
N
R
E
SU
L
T
S
I
n
t
h
is
s
ec
tio
n
,
d
y
n
a
m
ic
m
o
tio
n
o
f
t
h
e
t
w
o
-
li
n
k
m
an
ip
u
lato
r
is
s
i
m
u
lated
.
T
h
e
v
al
u
es
o
f
th
e
p
ar
am
eter
s
ar
e
g
iv
e
n
as
:
kg
m
2
1
,
kg
m
2
2
,
kg
m
p
1
,
m
L
1
1
an
d
m
L
1
2
.
T
o
s
im
u
late
t
h
e
o
p
ti
m
al
m
o
ti
o
n
o
f
t
h
e
m
an
ip
u
lato
r
,
it
i
s
as
s
u
m
ed
t
h
at
t
h
e
r
o
b
o
t
m
o
v
es
f
r
o
m
in
i
tial
p
o
s
itio
n
(
0
1
r
ad
,
12
/
2
r
ad
,
1
=0
r
ad
/s
,
2
=0
r
ad
/s
)
to
f
i
n
al
p
o
s
it
io
n
(
4
/
3
1
r
ad
,
2
/
2
r
ad
,
1
=0
r
ad
/s
,
2
=0
r
ad
/s
)
d
u
r
in
g
ti
m
e
o
f
s
t
f
2
.
1
.
T
h
e
p
ath
o
f
t
h
e
r
o
b
o
t
is
s
h
o
w
n
i
n
f
i
g
u
r
e
(
2
)
:
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
N
o
n
lin
ea
r
Dyn
a
mic
Mo
d
elin
g
a
n
d
Op
tima
l
Mo
tio
n
A
n
a
lysi
s
o
f Tw
o
-
Lin
k
Ma
n
ip
u
la
to
r
s
(M
.
Ta
leza
d
eh
)
65
Fig
u
r
e
2
.
p
ath
o
f
th
e
m
an
ip
u
la
to
r
As
it
is
s
ee
n
in
F
i
g
u
r
e
2
,
d
y
n
a
m
ic
m
o
tio
n
o
f
t
h
e
m
an
ip
u
lato
r
is
s
i
m
u
lated
r
eg
ar
d
i
n
g
t
o
d
er
iv
ed
n
o
n
li
n
ea
r
eq
u
atio
n
s
o
f
t
h
e
r
o
b
o
t.
Mo
r
e
o
v
er
,
th
e
an
g
u
lar
d
is
p
lace
m
e
n
ts
o
f
lin
k
s
o
f
th
e
r
o
b
o
t
a
r
e
s
h
o
w
n
in
f
i
g
u
r
es (
3
)
an
d
(
4
)
:
As
it
is
s
ee
n
i
n
ab
o
v
e
f
ig
u
r
es,
th
e
an
g
u
lar
d
is
p
lace
m
e
n
t
s
o
f
th
e
r
o
b
o
t
ar
e
s
m
o
o
th
.
F
u
r
th
e
r
m
o
r
e,
th
e
v
elo
cities o
f
th
e
r
i
g
h
t a
n
d
l
ef
t f
i
x
ed
w
h
ee
ls
o
f
th
e
r
o
b
o
t a
r
e
p
r
esen
ted
as:
Fig
u
r
e
3
.
An
g
u
lar
d
is
p
lace
m
e
n
t o
f
f
ir
s
t li
n
k
Fig
u
r
e
4
.
An
g
u
lar
d
is
p
lace
m
e
n
t o
f
s
ec
o
n
d
lin
k
Fig
u
r
e
5
.
s
p
ee
d
o
f
th
e
f
ir
s
t lin
k
o
f
t
h
e
r
o
b
o
t
Fig
u
r
e
6
.
s
p
ee
d
o
f
th
e
s
ec
o
n
d
l
in
k
As
it
is
s
ee
n
i
n
s
i
m
u
latio
n
s
tu
d
y
,
o
p
ti
m
a
l
d
y
n
a
m
ic
m
o
tio
n
o
f
th
e
m
a
n
ip
u
lato
r
is
s
i
m
u
lated
b
ased
o
n
d
er
iv
ed
eq
u
atio
n
s
o
f
t
h
e
s
y
s
te
m
.
5.
CO
NCLUS
I
O
N
I
n
th
i
s
ar
ticle
,
n
o
n
li
n
ea
r
d
y
n
a
m
ics
an
d
o
p
ti
m
al
m
o
tio
n
o
f
t
w
o
-
li
n
k
m
an
ip
u
lato
r
s
h
av
e
b
ee
n
in
v
e
s
ti
g
ated
.
T
o
d
y
n
a
m
ic
m
o
d
elin
g
o
f
th
e
s
y
s
te
m
,
t
h
e
L
ag
r
an
g
e
p
r
in
cip
le
h
as
b
ee
n
e
m
p
lo
y
ed
a
n
d
n
o
n
li
n
ea
r
d
y
n
a
m
ic
eq
u
atio
n
s
o
f
t
h
e
m
an
ip
u
lato
r
h
a
v
e
b
ee
n
p
r
esen
t
ed
in
s
tate
-
s
p
ac
e
f
o
r
m
.
T
h
en
,
o
p
ti
m
a
l
m
o
tio
n
an
al
y
s
is
o
f
t
h
e
n
o
n
li
n
ea
r
s
y
s
t
e
m
h
as
b
ee
n
d
e
v
elo
p
ed
b
ased
o
n
o
p
ti
m
al
co
n
tr
o
l
th
eo
r
y
.
B
y
m
ea
n
s
o
f
o
p
ti
m
al
co
n
tr
o
l
th
eo
r
y
,
in
d
ir
ec
t
s
o
l
u
ti
o
n
o
f
p
r
o
b
lem
h
as
b
ee
n
r
es
u
lt
ed
in
a
t
w
o
-
p
o
in
t
b
o
u
n
d
ar
y
v
alu
e
p
r
o
b
lem
w
h
ic
h
ca
n
b
e
s
o
l
v
ed
n
u
m
er
icall
y
.
F
i
n
all
y
,
i
n
o
r
d
er
to
d
e
m
o
n
s
tr
ate
th
e
p
o
w
er
an
d
e
f
f
icie
n
c
y
o
f
m
eth
o
d
,
a
n
u
m
b
er
o
f
s
i
m
u
lat
io
n
s
h
a
v
e
b
ee
n
p
er
f
o
r
m
ed
f
o
r
a
t
w
o
-
l
in
k
m
a
n
ip
u
l
ato
r
w
h
ic
h
s
h
o
w
ap
p
licab
ilit
y
o
f
th
e
p
r
o
p
o
s
ed
m
et
h
o
d
.
RE
F
E
R
E
NC
E
S
[1
]
P
i
lt
a
n
,
F
.
,
Ha
g
h
ig
h
i,
S
.
T
.
,
S
u
la
im
a
n
,
N.,
Na
z
a
ri,
I.
,
&
S
ia
m
a
k
,
S
.
(2
0
1
1
).
A
rti
f
icia
l
Co
n
tro
l
o
f
P
UMA
Ro
b
o
t
M
a
n
ip
u
lato
r:
A
-
Re
v
ie
w
o
f
F
u
z
z
y
In
f
e
re
n
c
e
En
g
in
e
A
n
d
A
p
p
li
c
a
ti
o
n
to
Clas
sic
a
l
Co
n
tr
o
ll
e
r.
I
n
ter
n
a
ti
o
n
a
l
J
o
u
rn
a
l
o
f
R
o
b
o
ti
c
s a
n
d
Au
t
o
ma
ti
o
n
,
2
(
5
),
4
0
1
-
4
2
5
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
9
-
4856
IJ
RA
Vo
l.
5
,
No
.
1
,
Ma
r
ch
2
0
1
6
:
6
1
–
6
6
66
[2
]
T
o
n
d
u
,
B.
,
I
p
p
o
li
t
o
,
S
.
,
G
u
io
c
h
e
t,
J.,
&
D
a
id
ie,
A
.
(2
0
0
5
)
.
A
se
v
e
n
-
d
e
g
re
e
s
-
of
-
f
re
e
d
o
m
ro
b
o
t
-
a
r
m
d
riv
e
n
b
y
p
n
e
u
m
a
ti
c
a
rti
f
icia
l
m
u
sc
le
s f
o
r
h
u
m
a
n
o
id
ro
b
o
ts.
T
h
e
In
ter
n
a
ti
o
n
a
l
J
o
u
rn
a
l
o
f
R
o
b
o
ti
c
s R
e
se
a
rc
h
,
24
(4
),
2
5
7
-
2
7
4
[3
]
R
ah
i
m
i,
H.
N.
,
&
Naz
e
m
izad
eh
,
M.
(
2
0
1
4
)
.
Dy
n
a
m
ic
an
al
y
s
i
s
an
d
in
tell
ig
e
n
t
co
n
tr
o
l
tech
n
iq
u
es
f
o
r
f
le
x
ib
le
m
a
n
ip
u
lato
r
s
: a
r
ev
ie
w
.
A
d
va
n
ce
d
R
o
b
o
tics
,
28
(
2
)
,
6
3
-
76
.
[4
]
Ko
ra
y
e
m
,
M
.
H.,
Na
z
e
m
i
z
a
d
e
h
,
M
.
,
&
Ra
h
im
i,
H.
N.
(2
0
1
3
).
T
ra
jec
to
r
y
o
p
ti
m
iza
ti
o
n
o
f
n
o
n
h
o
l
o
n
o
m
ic
m
o
b
il
e
m
a
n
ip
u
lat
o
rs d
e
p
a
rti
n
g
to
a
m
o
v
i
n
g
targ
e
t
a
m
id
st m
o
v
in
g
o
b
sta
c
le
s.
Acta
M
e
c
h
a
n
ica
,
2
2
4
(5
)
,
9
9
5
-
1
0
0
8
[5
]
Ko
ra
y
e
m
,
M
.
H.,
Ra
h
i
m
i,
H.
N.,
Nik
o
o
b
i
n
,
A
.
,
&
Na
z
e
m
i
z
a
d
e
h
,
M
.
(2
0
1
3
).
M
a
x
im
u
m
A
ll
o
w
a
b
le
D
y
n
a
m
ic
P
a
y
lo
a
d
f
o
r
F
lex
ib
le M
o
b
il
e
R
o
b
o
ti
c
M
a
n
i
p
u
lato
rs.
L
a
ti
n
Ame
ric
a
n
Ap
p
li
e
d
Res
e
a
rc
h
,
43
(
1
),
2
9
-
35
[6
]
Ko
ra
y
e
m
,
M
.
H.,
N
a
z
e
m
i
z
a
d
e
h
,
M
.
,
&
A
z
i
m
ir
a
d
,
V
.
(2
0
1
1
)
.
Op
ti
m
a
l
traje
c
to
r
y
p
lan
n
in
g
o
f
w
h
e
e
led
m
o
b
il
e
m
a
n
ip
u
lat
o
rs i
n
c
lu
tt
e
re
d
e
n
v
iro
n
m
e
n
ts
u
sin
g
p
o
ten
ti
a
l
f
u
n
c
ti
o
n
s.
S
c
ien
ti
a
Ira
n
ica
,
18
(5
)
,
1
1
3
8
-
1
1
4
7
[7
]
L
u
h
,
J.Y.S
.
,
"
Co
n
v
e
n
ti
o
n
a
l
c
o
n
tr
o
ll
e
r
d
e
sig
n
f
o
r
in
d
u
strial
ro
b
o
ts
—
A
tu
to
rial,
"
S
y
ste
ms
,
M
a
n
a
n
d
Cy
b
e
rn
e
ti
c
s,
IEE
E
T
ra
n
sa
c
ti
o
n
s
o
n
,
v
o
l.
S
M
C
-
1
3
,
n
o
.
3
,
p
p
.
2
9
8
,
3
1
6
,
M
a
y
-
Ju
n
e
1
9
8
3
[8
]
S
o
n
g
,
Z
.
,
Yi,
J.
,
Zh
a
o
,
D.,
&
L
i,
X
.
(2
0
0
5
).
A
c
o
m
p
u
ted
to
r
q
u
e
c
o
n
tr
o
ll
e
r
f
o
r
u
n
c
e
rtain
ro
b
o
ti
c
m
a
n
ip
u
lato
r
s
y
ste
m
s: F
u
z
z
y
a
p
p
ro
a
c
h
.
Fu
zz
y
S
e
ts
a
n
d
S
y
ste
ms
,
1
5
4
(
2
),
2
0
8
-
2
2
6
[9
]
P
i
lt
a
n
,
F
.
,
M
e
h
ra
ra
,
S
.
,
Ba
y
a
t,
R.
,
&
Ra
h
m
d
e
l,
S
.
(2
0
1
2
)
.
De
sig
n
Ne
w
Co
n
tro
l
M
e
th
o
d
o
lo
g
y
o
f
In
d
u
strial
R
o
b
o
t
M
a
n
ip
u
lato
r:
S
li
d
in
g
M
o
d
e
Ba
se
l
in
e
M
e
th
o
d
o
lo
g
y
[1
0
]
Ko
ra
y
e
m
,
M
.
H.,
&
No
h
o
o
j
i,
H.
R.
(2
0
0
8
).
T
ra
jec
to
r
y
o
p
ti
m
iz
a
ti
o
n
o
f
f
le
x
ib
le
m
o
b
il
e
m
a
n
ip
u
lato
rs
u
sin
g
o
p
e
n
-
lo
o
p
o
p
ti
m
a
l
c
o
n
tro
l
m
e
th
o
d
.
I
n
I
n
telli
g
e
n
t
R
o
b
o
ti
c
s
a
n
d
Ap
p
li
c
a
ti
o
n
s
(
p
p
.
5
4
-
6
3
).
S
p
ri
n
g
e
r
Be
rli
n
He
id
e
lb
e
rg
[1
1
]
Ko
ra
y
e
m
,
M
.
H.,
Ha
g
h
p
a
n
a
h
i,
M
.
,
Ra
h
im
i,
H.
N.,
&
Nik
o
o
b
i
n
,
A
.
(2
0
0
9
)
.
F
i
n
it
e
e
lem
e
n
t
m
e
th
o
d
a
n
d
o
p
ti
m
a
l
c
o
n
tro
l
th
e
o
ry
f
o
r
p
a
th
p
lan
n
in
g
o
f
e
las
ti
c
m
a
n
ip
u
lato
rs.
I
n
Ne
w
Ad
v
a
n
c
e
s
in
In
telli
g
e
n
t
De
c
is
io
n
T
e
c
h
n
o
l
o
g
ies
(p
p
.
117
-
1
2
6
).
S
p
ri
n
g
e
r
Be
rli
n
He
id
e
l
b
e
rg
[1
2
]
Ko
ra
y
e
m
,
M
.
H.,
Na
z
e
m
iza
d
e
h
,
M
.
,
&
No
h
o
o
ji
,
H.
R
.
(2
0
1
2
).
S
m
o
o
th
jerk
-
b
o
u
n
d
e
d
o
p
ti
m
a
l
p
a
th
p
lan
n
in
g
o
f
tri
c
y
c
le
w
h
e
e
led
m
o
b
il
e
m
a
n
ip
u
la
to
rs i
n
th
e
p
re
se
n
c
e
o
f
e
n
v
iro
n
m
e
n
tal
o
b
sta
c
les
.
In
t
J
A
d
v
R
o
b
o
ti
c
S
y
,
9
(1
0
5
)
[1
3
]
Ko
ra
y
e
m
,
M
.
H.,
Na
z
e
m
iza
d
e
h
,
M
.
,
&
Ra
h
im
i,
H.
N.
(
2
0
1
4
)
.
Dy
n
a
m
ic
o
p
ti
m
a
l
p
a
y
lo
a
d
p
a
th
p
lan
n
in
g
o
f
m
o
b
il
e
m
a
n
ip
u
lat
o
rs am
o
n
g
m
o
v
in
g
o
b
st
a
c
les
.
Ad
v
a
n
c
e
d
R
o
b
o
ti
c
s
,
28
(2
0
),
1
3
8
9
-
1
4
0
2
[1
4
]
Ko
ra
y
e
m
,
M
.
H.,
Na
z
e
m
iza
d
e
h
,
M
.
,
&
No
h
o
o
ji
,
H.
R
.
(
2
0
1
4
).
Op
ti
m
a
l
p
o
in
t
-
to
-
p
o
i
n
t
m
o
ti
o
n
p
lan
n
i
n
g
o
f
n
o
n
-
h
o
l
o
n
o
m
ic
m
o
b
il
e
ro
b
o
ts
in
th
e
p
re
se
n
c
e
o
f
m
u
lt
ip
le
o
b
sta
c
les
.
J
o
u
rn
a
l
o
f
t
h
e
Bra
zili
a
n
S
o
c
iety
o
f
M
e
c
h
a
n
ica
l
S
c
ien
c
e
s a
n
d
En
g
i
n
e
e
rin
g
,
36
(1
),
2
2
1
-
2
3
2
Evaluation Warning : The document was created with Spire.PDF for Python.