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.
4
,
No
.
4
,
Dec
em
b
er
201
5
,
p
p
.
2
84
~
2
91
I
SS
N:
2089
-
4856
284
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
O
P
P
Ap
pro
a
ch f
o
r Multi
Deg
ree
o
f
Free
do
m
Ro
bo
tic
Ar
m
Ba
sed o
n K
ine
m
a
tics and Dy
na
m
ic
s o
f
Ro
bo
t
Ra
s
h
m
i
ra
nja
n Da
s
*
,
Rupa
li
M
a
t
hu
r
*
,
Dee
pik
a
Ra
ni So
n
a
**
,
P
r
a
neet
Dut
t
a
*
*
*
S
c
h
o
o
l
o
f
El
e
c
tri
c
a
l
En
g
in
e
e
rin
g
,
V
IT
Un
iv
e
rsity
,
T
a
m
il
Na
d
u
,
In
d
ia
*
*
S
c
h
o
o
l
o
f
El
e
c
tro
n
ic E
n
g
in
e
e
rin
g
,
V
IT
Un
iv
e
rsity
,
T
a
m
il
Na
d
u
,
In
d
ia
Art
icle
I
nfo
AB
ST
RAC
T
A
r
ticle
his
to
r
y:
R
ec
eiv
ed
J
u
l
1
0
,
2
0
1
5
R
ev
i
s
ed
Oct
2
5
,
2
0
1
5
A
cc
ep
ted
No
v
1
4
,
2
0
1
5
T
h
is
p
a
p
e
r
d
e
a
ls
w
it
h
th
e
traje
c
to
ry
a
n
d
p
a
th
g
e
n
e
ra
ti
o
n
o
f
th
e
in
d
u
strial
m
a
n
ip
u
lat
o
r.
T
h
e
traje
c
to
r
y
is
o
b
tain
e
d
u
si
n
g
th
e
e
q
u
a
ti
o
n
s
o
f
m
o
ti
o
n
a
n
d
a
lso
th
e
o
p
ti
m
a
l
p
a
th
p
lan
n
i
n
g
(OPP
)
a
p
p
ro
a
c
h
u
n
d
e
r
k
i
n
o
d
y
n
a
m
i
c
c
o
n
stra
in
ts.
T
h
e
o
p
ti
m
a
l
c
o
n
tro
l
p
ro
b
lem
is
d
e
f
in
e
d
f
o
r
th
e
m
in
im
u
m
c
o
st
f
u
n
c
ti
o
n
a
n
d
to
o
b
tai
n
th
e
n
e
c
e
ss
a
r
y
c
o
n
d
it
io
n
s.
He
re
we
h
a
v
e
u
se
d
p
o
n
try
g
a
in
’s
m
in
im
u
m
p
rin
c
ip
le
to
o
b
tain
th
e
li
m
it
in
g
v
a
lu
e
o
f
jo
in
t
a
n
g
le
a
n
d
a
lso
th
e
jo
in
t
v
e
lo
c
it
y
a
n
d
to
rq
u
e
.
In
th
is
p
a
p
e
r
w
e
h
a
v
e
u
se
d
th
e
“
Tw
o
d
e
g
re
e
o
f
f
re
e
d
o
m
(DO
F
)
m
a
n
ip
u
lat
o
r”
f
o
r
a
n
a
ly
sis
a
n
d
d
e
sig
n
in
g
th
e
o
p
ti
m
a
l
c
o
n
tro
l
f
o
r
m
u
lt
i
li
n
k
a
n
d
m
u
lt
i
d
e
g
re
e
o
f
f
re
e
d
o
m
m
a
n
ip
u
lato
r.
F
o
r
a
n
a
ly
sis
p
u
rp
o
se
s,
sim
u
latio
n
so
f
t
w
a
re
h
a
s
b
e
e
n
u
se
d
to
f
o
rm
u
late
th
e
traje
c
to
ry
a
n
d
m
in
i
m
ize
th
e
c
o
st f
u
n
c
ti
o
n
i
n
v
o
lv
e
d
.
K
ey
w
o
r
d:
OP
P
P
o
n
tr
y
g
ai
n
’
s
p
r
in
cip
le
C
o
s
t f
u
n
ctio
n
k
in
o
d
y
n
a
m
ic
co
n
s
tr
ain
s
DOF
Co
p
y
rig
h
t
©
201
5
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
:
R
u
p
ali
Ma
t
h
u
r
Sch
o
o
l o
f
E
lectr
ical
E
n
g
i
n
ee
r
i
n
g
VI
T
Un
iv
er
s
it
y
Vello
r
e
6
3
2
0
1
4
,
T
am
il Na
d
u
,
I
n
d
ia
r
u
p
ali
m
at
h
u
r
8
@
g
m
a
il.c
o
m
Dee
p
ik
a
R
a
n
i So
n
a
Sch
o
o
l o
f
E
lectr
o
n
ics
E
n
g
i
n
ee
r
i
n
g
VI
T
Un
iv
er
s
it
y
Vello
r
e
6
3
2
0
1
4
,
T
am
il Na
d
u
,
I
n
d
ia
d
ee
p
ik
a.
r
s
@
v
it.a
c.
in
1.
I
NT
RO
DUC
T
I
O
N
I
n
a
p
leth
o
r
a
o
f
in
d
u
s
tr
ie
s
,
R
o
b
o
tics
is
a
n
i
n
teg
r
al
p
ar
t
o
f
a
u
to
m
at
io
n
.
T
h
e
i
n
d
u
s
tr
ia
l
r
o
b
o
tic
ar
m
is
u
s
ed
f
o
r
v
ar
io
u
s
ta
s
k
s
r
i
g
h
t
f
r
o
m
th
e
a
s
s
e
m
b
l
y
,
s
p
r
a
y
p
ai
n
t
in
g
an
d
w
e
ld
in
g
,
ca
r
r
y
i
n
g
p
ar
ts
f
r
o
m
o
n
e
p
lace
to
an
o
th
er
,
p
ac
k
ag
i
n
g
an
d
m
a
n
y
o
t
h
er
task
s
.
Fo
r
estab
lis
h
i
n
g
a
m
a
n
ip
u
lato
r
it
is
r
eq
u
ir
ed
to
estab
lis
h
th
e
tr
aj
ec
to
r
y
p
ath
.
W
ith
o
u
t
a
n
y
co
n
s
tr
ai
n
ts
a
n
d
th
e
p
ath
t
h
e
i
n
d
u
s
tr
y
ca
n
n
o
tg
o
ah
ea
d
w
it
h
t
h
e
p
r
o
d
u
ctio
n
p
r
o
ce
s
s
ef
f
icien
tl
y
.
T
o
o
p
tim
ize
o
u
r
s
o
lu
tio
n
,
w
e
a
n
al
y
ze
th
e
co
n
s
tr
ai
n
ts
o
n
t
h
e
j
o
in
t a
n
g
le
,
j
o
in
t v
el
o
cit
y
an
d
to
r
q
u
e.
I
f
t
h
e
r
o
b
o
tic
ar
m
w
er
e
to
m
o
v
e
in
a
f
r
ee
tr
aj
ec
to
r
y
it
is
ca
p
ab
le
o
f
d
estro
y
in
g
lar
g
e
p
r
o
p
o
r
tio
n
s
o
f
th
e
w
o
r
k
s
p
ac
e.
As
s
h
o
w
n
i
n
f
i
g
-
An
a
u
to
m
o
ti
v
e
i
n
d
u
s
tr
y
i
n
w
h
ich
ca
r
i
s
ass
e
m
b
led
u
s
i
n
g
t
h
e
r
o
b
o
tic
ar
m
.
E
ac
h
an
d
ev
er
y
r
o
b
o
ti
c
m
an
ip
u
lato
r
h
as
it
s
o
w
n
tr
aj
ec
to
r
y
d
ef
i
n
ed
.
I
f
th
e
p
ath
h
a
s
n
o
t
b
ee
n
d
ec
id
ed
,
th
en
th
e
tr
aj
ec
to
r
y
o
f
t
h
e
r
o
b
o
t is lo
s
t a
n
d
th
e
d
a
m
a
g
e
h
as b
ee
n
i
n
f
lict
ed
b
y
th
e
m
a
n
ip
u
lato
r
w
h
ic
h
i
s
d
an
g
er
o
u
s
f
o
r
t
h
e
in
d
u
s
tr
ial
en
v
ir
o
n
m
en
t
an
d
p
eo
p
le
w
o
r
k
i
n
g
in
t
h
e
in
d
u
s
t
r
y
.
Her
e
w
e
u
s
e
t
h
e
k
i
n
o
d
y
n
a
m
ic
co
n
s
tr
ai
n
t
s
to
f
o
r
m
u
late
th
e
tr
aj
ec
to
r
y
.
K
in
o
d
y
n
a
m
ic
s
r
e
f
er
s
to
k
i
n
e
m
at
ics
a
n
d
d
y
n
a
m
ic
f
o
r
m
u
latio
n
.
Kin
e
m
at
ics
o
f
th
e
r
o
b
o
t
in
v
o
lv
es
t
h
e
f
o
r
m
u
lati
o
n
o
f
D
-
H
p
ar
a
m
eter
alo
n
g
w
it
h
t
h
e
j
ac
o
b
ian
m
atr
ix
.
T
h
e
j
ac
o
b
ian
m
a
tr
ix
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
OOP
A
p
p
r
o
a
ch
f
o
r
Mu
lti De
g
r
ee
o
f F
r
ee
d
o
m
R
o
b
o
tic
A
r
m
b
a
s
ed
o
n
K
in
ema
tics
.
.
.
(
R
a
s
h
m
ir
a
n
ja
n
Da
s
)
285
in
v
o
l
v
es
t
h
e
r
o
tatio
n
al
an
d
tr
an
s
lat
io
n
o
f
t
h
e
r
o
b
o
tic
ar
m
.
T
r
a
j
ec
to
r
y
g
e
n
er
atio
n
i
n
v
o
lv
e
t
w
o
m
et
h
o
d
T
h
e
Fo
r
m
er
is
t
h
e
d
ec
o
u
p
led
ap
p
r
o
ac
h
in
w
h
ic
h
a
f
ea
s
ib
le
p
ath
is
f
ir
s
t
f
o
r
m
ed
a
n
d
th
e
n
o
p
ti
m
izatio
n
o
f
co
n
tr
o
l
alo
n
g
t
h
e
p
ath
ta
k
es
p
lace
.
T
h
e
Seco
n
d
m
et
h
o
d
in
v
o
l
v
es
a
d
i
r
ec
t
tr
aj
ec
to
r
y
p
lan
n
i
n
g
ap
p
r
o
ac
h
-
I
t
i
n
v
o
lv
e
s
th
e
k
in
o
d
y
n
a
m
ic
co
n
s
tr
ai
n
t
s
o
f
t
h
e
s
y
s
te
m
to
s
o
lv
e
th
e
o
p
tim
al
p
at
h
p
la
n
n
in
g
ap
p
r
o
ac
h
.
Dir
ec
t
tr
aj
ec
to
r
y
p
lan
n
i
n
g
ap
p
r
o
ac
h
is
d
o
n
e
b
y
u
s
i
n
g
th
e
eq
u
atio
n
s
o
f
m
o
t
io
n
.
I
n
th
i
s
ap
p
r
o
ac
h
,
th
e
o
p
ti
m
al
s
o
lu
tio
n
is
d
er
iv
ed
f
r
o
m
t
h
e
s
tate
s
p
ac
e
o
f
th
e
s
y
s
te
m
an
d
it
s
o
lv
es
an
u
n
k
n
o
w
n
o
p
ti
m
al
tr
aj
ec
to
r
y
o
f
ea
c
h
j
o
in
t.I
n
th
i
s
Op
ti
m
al
co
n
tr
o
l
p
r
o
b
lem
,
w
e
w
ill
f
ir
s
t
s
o
l
v
e
t
h
e
t
w
o
d
eg
r
ee
o
f
f
r
ee
d
o
m
r
o
b
o
tic
m
a
n
ip
u
lato
r
.
Hen
ce
,
u
s
in
g
t
h
e
co
n
s
tr
ain
ts
w
e
w
il
l d
ef
i
n
e
th
e
tr
aj
ec
to
r
y
o
f
th
e
r
o
b
o
tic
ar
m
.
Fig
u
r
e
1
.
Au
to
m
o
ti
v
e
i
n
d
u
s
tr
y
2.
B
L
O
CK
DIAG
R
AM
T
h
e
b
lo
ck
d
iag
r
am
u
s
ed
to
ca
l
cu
late
t
h
e
tr
aj
ec
to
r
y
o
f
t
h
e
p
ath
is
:
Fig
u
r
e
2
.
B
lo
ck
d
iag
r
a
m
A
cc
o
r
d
in
g
to
th
is
,
in
itiall
y
t
h
e
r
o
b
o
t
g
eo
m
etr
y
i
s
d
ef
in
e
d
alo
n
g
w
it
h
r
o
b
o
t
d
y
n
a
m
ic
s
s
u
c
h
as
v
elo
cit
y
,
k
in
et
ic
a
n
d
p
o
ten
tial
en
er
g
y
o
f
th
e
r
o
b
o
tic
j
o
in
t.
T
h
en
w
e
f
o
r
m
u
late
t
h
e
o
p
ti
m
a
l
co
n
tr
o
l
al
g
o
r
ith
m
.
Usi
n
g
t
h
e
tr
aj
ec
to
r
y
eq
u
atio
n
,
th
e
f
o
llo
w
i
n
g
ar
e
also
s
et
-
th
e
o
p
ti
m
al
co
n
s
tr
ai
n
t
s
o
n
th
e
j
o
in
t
an
g
le,
j
o
in
t
v
elo
cit
y
,
f
o
r
ce
an
d
to
r
q
u
e.
T
h
ese
co
n
s
tr
ai
n
t
s
ar
e
ap
p
licab
le
o
n
th
e
o
p
ti
m
al
p
at
h
.
T
h
is
tr
aj
ec
to
r
y
i
s
d
ef
in
ed
i
n
th
e
co
n
t
r
o
l
o
f
th
e
r
o
b
o
tic
m
a
n
ip
u
lato
r
.
E
q
u
atio
n
s
o
f
m
o
tio
n
ar
e
ap
p
lied
t
o
p
lan
th
e
tr
aj
ec
t
o
r
y
a
n
d
p
o
n
tr
y
g
ai
n
p
r
in
ci
p
le
to
o
b
tain
th
e
o
p
ti
m
al
v
alu
e
o
f
th
e
j
o
in
t a
n
g
le
v
elo
ci
t
y
an
d
ac
ce
ler
atio
n
.
3.
E
Q
UA
T
I
O
N
S AN
D
CA
L
C
UL
A
T
I
O
NS
E
q
u
atio
n
o
f
Mo
tio
n
:
D
(
q
)
̈
+C(q
,
̇
̇
+g
(
q
)
=Q
(
1
)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
9
-
4856
IJ
RA
Vo
l.
4
,
No
.
4
,
Dec
em
b
er
201
5
:
2
84
–
2
91
286
W
h
er
e
q
is
t
h
e
v
ec
to
r
o
f
g
en
er
alize
d
co
o
r
d
in
ates(
θ
1
,θ
2
in
t
h
is
ca
s
e)
,
D
i
s
a
p
o
s
iti
v
e
d
ef
i
n
ite
i
n
er
tia
m
atr
i
x
,
C
is
th
e
v
elo
cit
y
co
u
p
lin
g
v
ec
to
r
,
g
is
th
e
g
r
av
i
tat
io
n
f
o
r
ce
v
ec
to
r
,
an
d
Q
is
th
e
g
en
er
alize
d
f
o
r
ce
v
ec
to
r
o
r
th
e
j
o
in
t to
r
q
u
es τ
1,
τ
2
in
th
is
ca
s
e.
P
er
f
o
r
m
a
n
ce
m
ea
s
u
r
e
is
g
i
v
e
n
b
y
J
=0
.
5
*
x
’
P
x
+0
.
5
∫
w
h
er
e
P
an
d
Q
ar
e
p
o
s
itiv
e
s
e
m
i
d
ef
i
n
ite
m
a
tr
ix
an
d
R
i
s
p
o
s
itiv
e
d
ef
i
n
ite
m
atr
i
x
.
X
is
th
e
s
tate
v
ec
to
r
a
n
d
U
i
s
t
h
e
co
n
tr
o
l
v
ec
to
r
.
Fo
r
o
p
tim
a
l
co
n
tr
o
l
p
r
o
b
lem
w
e
f
o
llo
w
th
r
ee
s
tep
s
to
o
b
tain
th
e
o
p
tim
al
v
alu
e
o
f
j
o
in
t
an
g
le
θ
.
T
h
e
f
ir
s
t
in
v
o
l
v
es
th
e
p
r
o
b
le
m
f
o
r
m
u
latio
n
w
h
ic
h
i
s
d
ef
i
n
ed
b
y
t
h
e
k
in
e
m
at
ics
o
f
t
h
e
r
o
b
o
t
ar
m
.
I
n
itiall
y
w
e
tak
e
t
w
o
d
e
g
r
ee
o
f
f
r
ee
d
o
m
f
o
r
th
e
r
o
b
o
tics
ar
m
t
o
f
o
r
m
u
late
t
h
e
eq
u
at
io
n
s
.
T
h
e
s
ec
o
n
d
s
tep
co
n
s
is
ts
o
f
d
ef
i
n
i
n
g
th
e
co
n
s
tr
ai
n
t
s
-
ap
p
ly
i
n
g
p
h
y
s
ical
co
n
s
tr
ain
t
s
to
th
e
r
o
b
o
tic
ar
m
.
T
h
ir
d
is
th
e
f
o
r
m
u
lat
io
n
o
f
t
h
e
p
er
f
o
r
m
an
ce
m
ea
s
u
r
es
to
o
b
tain
th
e
o
p
ti
m
al
v
al
u
e
o
f
j
o
in
t a
n
g
le.
C
o
n
s
id
er
th
e
t
w
o
d
eg
r
ee
o
f
f
r
ee
d
o
m
r
o
b
o
tic
ar
m
as
s
h
o
w
n
in
f
i
g
u
r
e.
Def
i
n
i
n
g
t
h
e
k
i
n
e
m
a
tic
p
ar
a
m
eter
a
1
an
d
a
2
ar
e
th
e
lin
k
len
g
t
h
d
1
an
d
d
2
a
r
e
th
e
lin
k
o
f
f
s
et
s
,
θ
1
an
d
θ
2
ar
e
j
o
in
t
an
g
le.
x
,
y
,
z
d
e
f
in
e
th
e
r
o
tatio
n
al
a
x
i
s
.
2
-
D
OF
li
n
k
m
an
ip
u
lato
r
h
as
t
h
e
en
d
a
f
f
ec
ter
o
f
d
if
f
er
en
t
s
h
ap
es it c
a
n
b
e
a
w
eld
i
n
g
to
r
c
h
o
r
a
p
ain
t b
r
u
s
h
,
cla
m
p
er
etc.
Fig
u
r
e
3
.
T
w
o
d
eg
r
ee
o
f
f
r
ee
d
o
m
r
o
b
o
tic
ar
m
A
p
p
l
y
t
h
e
Den
a
v
it
-
Har
te
n
b
er
g
h
o
m
o
-
g
en
eo
u
s
tr
an
s
f
er
m
a
tr
ix
u
s
ed
to
ch
ar
ac
ter
ize
th
e
k
in
e
m
atic
s
o
f
r
o
b
o
tic
ar
m
.
Usi
n
g
r
o
tatio
n
s
a
n
d
tr
an
s
lat
io
n
al
[
]
=
[
]
(
[
]
+
[
]
[
]
)
-
(
2
)
x
=
a
1
+ d
2
+ a
2
(
3
)
y
=a
1
–
d
2
+ a
2
(
4
)
z=d
1
+ a
2
(
5
)
T
h
ese
eq
u
atio
n
s
ar
e
u
s
ed
f
o
r
ca
lcu
lat
in
g
t
h
e
v
al
u
es
o
f
t
h
e
co
n
s
tr
ai
n
ts
an
d
p
er
f
o
r
m
an
ce
m
ea
s
u
r
es
.
A
p
p
l
y
in
g
t
h
e
t
w
o
d
eg
r
ee
o
f
f
r
ee
d
o
m
d
i
m
e
n
s
io
n
s
a
n
d
li
m
its
as
f
o
r
li
n
k
1
t
h
at
i
s
a
1
=0
.
0
m
,
d
1
=1
0
cm
,
f
o
r
li
n
k
2
a
2
=4
8
cm
,
d
2
=2
c
m
.
B
y
ca
lc
u
l
atio
n
w
e
o
b
tain
th
e
v
al
u
e
o
f
to
r
q
u
e,
an
g
u
lar
v
elo
cit
y
a
n
d
ac
ce
ler
atio
n
.
A
s
f
o
r
lin
k
1
it is
τ
1
=2
.
4
N
m
,
τ
2
=2
.
9
N
m
,
ω
1
=7
.
5
r
ad
/s
ec
,
ω
2
=7
.
5
r
ad
/s
ec
,
α
1
=7
.
5
r
ad
/s
ec
2
α
2
=1
0
r
ad
/s
ec
2
.
Fo
r
m
in
g
t
h
e
co
n
s
tr
ain
t
eq
u
ati
o
n
s
b
y
ap
p
l
y
i
n
g
th
e
f
o
llo
w
i
n
g
co
n
d
itio
n
s
o
n
s
tate
v
ec
to
r
an
d
co
n
tr
o
l
v
ec
to
r
as:
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
OOP
A
p
p
r
o
a
ch
f
o
r
Mu
lti De
g
r
ee
o
f F
r
ee
d
o
m
R
o
b
o
tic
A
r
m
b
a
s
ed
o
n
K
in
ema
tics
.
.
.
(
R
a
s
h
m
ir
a
n
ja
n
Da
s
)
287
X=
[
̇
̇
]
ε
X
{
̇
̇
̇
̇
}
(
6
)
C
o
n
tr
o
l v
ec
to
r
u
=
[
]
T
ε
U
{
}
(
7
)
Min
i
m
ize
J
[
x
(
)
,
u
(
)
,
t
f
]
=t
f
Su
b
j
ec
t to
:
̇
(
t)
=
[
̇
̇
]
=
[
]
w
h
er
e
α
i
=f
(
x
,
u
)
x
(t
0
)
=
[
]
(
8
)
e(
x
f
,t
f
)=
[
]
[
]
(
9
)
[
]
[
]
(
1
0
)
No
w
ap
p
l
y
in
g
t
h
e
n
ec
e
s
s
ar
y
co
n
d
itio
n
s
-
T
o
b
e
o
p
tim
al
,
th
e
s
o
lu
t
io
n
m
u
s
t
s
ati
s
f
y
th
e
p
o
n
tr
y
g
ai
n
’
s
p
r
in
cip
le.
Def
in
e
t
h
e
Ha
m
ilto
n
ian
,
H
as
a
f
u
n
c
tio
n
o
f
r
u
n
n
in
g
co
s
t,
F,
th
e
v
ec
to
r
o
f
co
-
s
tates
λ
(
t)
,
an
d
th
e
p
r
in
cip
le
as:
H
(
λ
,
x
,
u
,
t)
F+λ
T
f
(
1
1
)
3
.
1
.
SCE
NAR
I
O
1
No
w
ap
p
l
y
i
n
g
t
h
e
g
i
v
en
ab
o
v
e
co
n
f
ig
u
r
atio
n
s
an
d
ca
lcu
la
ti
n
g
it
w
it
h
th
e
s
i
m
u
lat
io
n
an
d
o
b
tain
in
g
th
e
r
esu
lts
ap
p
l
y
i
n
g
th
e
1
st
s
ce
n
ar
io
as g
i
v
en
:
=0
,
=9
0
,
=4
5
,
=4
5
th
e
r
esu
lt f
o
r
th
is
is
a
s
Fig
u
r
e
4a
.
L
in
k
1
p
ar
a
m
eter
o
f
E
n
d
X
Fig
u
r
e
4
b
.
L
in
k
1
&
li
n
k
2
p
ar
am
eter
en
d
y
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
9
-
4856
IJ
RA
Vo
l.
4
,
No
.
4
,
Dec
em
b
er
201
5
:
2
84
–
2
91
288
Fig
u
r
e
4
c.
L
in
k
1
&
lin
k
2
p
ar
am
eter
en
d
z
Fig
u
r
e
5
.
J
o
in
t v
alu
e
o
f
s
ce
n
ar
io
1
I
n
th
e
j
o
in
t
v
elo
cit
y
p
lo
t
f
o
r
l
in
k
1
&
li
n
k
2
th
e
c
y
clo
id
tr
aj
ec
to
r
y
is
r
ep
r
ese
n
ted
as
in
t
h
e
s
h
ap
e
o
f
c
y
clo
id
.
Fro
m
th
e
f
i
g
u
r
e
w
e
ca
n
s
ee
t
h
at
th
e
s
h
ap
e
o
f
t
h
e
cu
r
v
e
is
v
ar
y
i
n
g
at
d
i
f
f
er
e
n
t
v
alu
es o
f
j
o
in
t v
elo
cit
y
.
J
o
in
t
ac
ce
ler
atio
n
w
ith
t
h
e
i
n
v
er
s
e
d
y
n
a
m
ic
co
n
s
tr
ain
t
s
g
iv
e
s
th
is
p
lo
t.
Fig
u
r
e
5
a.
J
o
in
t v
elo
cit
y
o
f
l
in
k
1
an
d
lin
k
2
Fig
u
r
e
6
.
J
o
in
t a
cc
eler
atio
n
o
f
lin
k
1
&
li
n
k
2
Fo
r
ce
an
d
to
r
q
u
e
w
it
h
th
e
i
n
v
e
r
s
e
k
i
n
e
m
at
ic
co
n
s
tr
ai
n
t
s
.
Fig
u
r
e
7.
Fo
r
ce
/to
r
q
u
e
o
f
th
e
jo
in
t 1
&
j
o
in
t 2
3
.
2
.
SCE
NAR
I
O
2
No
w
ca
lc
u
lati
n
g
t
h
e
co
n
s
tr
ain
t
s
v
al
u
e
in
t
h
e
s
ec
o
n
d
s
ce
n
ar
i
o
=
-
90
֯
,
=1
8
0
֯
,
=
-
90
֯
,
=9
0
֯
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
OOP
A
p
p
r
o
a
ch
f
o
r
Mu
lti De
g
r
ee
o
f F
r
ee
d
o
m
R
o
b
o
tic
A
r
m
b
a
s
ed
o
n
K
in
ema
tics
.
.
.
(
R
a
s
h
m
ir
a
n
ja
n
Da
s
)
289
T
h
e
v
alu
e
o
f
j
o
in
t a
n
g
le
a
n
d
j
o
in
t v
e
lo
cit
y
w
i
ll v
ar
y
in
t
h
i
s
s
c
en
ar
io
b
ec
au
s
e
th
e
tr
aj
ec
to
r
y
it
f
o
r
m
s
w
il
l b
e
a
cir
cu
lar
o
n
e
s
tar
tin
g
f
r
o
m
o
n
e
e
n
d
an
d
en
d
in
g
at
t
h
e
o
th
er
.
Fig
u
r
e
8
a.
L
in
k
1
&
l
in
k
2
x
-
a
x
is
p
lo
t
Fig
u
r
e
8
b
.
Y
ax
is
p
lo
t
Fig
u
r
e
8
c.
Z
ax
i
s
p
lo
t
T
h
e
j
o
in
t
an
g
le
p
lo
t
,
j
o
in
t
v
el
o
cit
y
p
lo
t,
an
d
j
o
in
t
ac
ce
ler
ati
o
n
p
lo
t
is
g
i
v
en
a
s
f
o
r
b
o
th
th
e
lin
k
1
&
lin
k
2
:
Fig
u
r
e
9
.
J
o
in
t a
n
g
le
p
lo
t o
f
s
c
en
ar
io
2
Fig
u
r
e
1
0
.
J
o
in
t v
elo
cit
y
o
f
s
c
en
ar
io
2
Fig
u
r
e
1
1
.
J
o
in
t a
cc
eler
atio
n
o
f
s
ce
n
ar
io
2
Fig
u
r
e
1
2
.
Fo
r
ce
&
to
r
q
u
e
o
f
s
ce
n
ar
io
2
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
9
-
4856
IJ
RA
Vo
l.
4
,
No
.
4
,
Dec
em
b
er
201
5
:
2
84
–
2
91
290
4.
RE
SU
L
T
W
e
h
av
e
co
m
p
ar
ed
t
h
e
t
w
o
s
ce
n
ar
io
s
a
n
d
h
av
e
o
b
tain
ed
t
h
e
f
o
llo
w
i
n
g
r
es
u
lt
s
.
I
n
th
e
f
ir
s
t
s
ce
n
ar
io
th
e
tr
aj
ec
to
r
y
i
s
g
en
er
ated
as
a
q
u
ar
ter
cir
cle
an
d
i
n
t
h
e
s
ec
o
n
d
s
ce
n
ar
io
t
h
e
tr
aj
ec
to
r
y
i
s
g
en
er
ated
as
a
f
u
ll
cir
cle
s
tar
tin
g
en
d
i
n
g
at
t
h
e
s
a
m
e
p
o
in
t.
Ta
in
g
th
e
m
ea
n
o
f
th
e
li
n
k
v
al
u
es
a
n
d
co
n
s
o
lid
atin
g
it i
n
to
a
tab
u
lar
f
o
r
m
w
e
g
et:
T
ab
le
1
.
C
o
m
p
ar
is
o
n
b
et
w
ee
n
t
h
e
v
al
u
es o
f
s
ce
n
ar
io
1
&
s
ce
n
ar
io
2
C
h
a
r
a
c
t
e
r
i
st
i
c
S
c
e
n
a
r
i
o
1
S
c
e
n
a
r
i
o
2
L
i
n
k
x
-
a
x
i
s
0
.
5
4
m
0
.
5
0
m
L
i
n
k
y
-
a
x
i
s
0
.
2
9
m
0
.
1
5
m
L
i
n
k
z
-
a
x
i
s
0
.
0
0
2
2
m
0
.
0
0
1
2
m
Jo
i
n
t
a
n
g
l
e
4
5
d
e
g
5
0
d
e
g
Jo
i
n
t
v
e
l
o
c
i
t
y
9
0
d
e
g
/
se
c
1
0
0
d
e
g
/
se
c
Jo
i
n
t
a
c
c
e
l
e
r
a
t
i
o
n
3
4
0
d
e
g
/
se
c
2
3
0
0
d
e
g
/
se
c
2
F
o
r
c
e
&
t
o
r
q
u
e
0
.
1
7
5
7
N
m
0
.
0
9
9
5
N
m
A
s
th
e
tr
aj
ec
to
r
y
g
e
n
er
ated
i
s
also
d
if
f
er
en
t
in
th
e
s
e
ca
s
e
s
-
O
n
e
is
q
u
ar
ter
s
h
ap
ed
an
d
t
h
e
o
th
er
i
s
cir
cu
lar
.
Fig
u
r
e
1
3
.
Scen
ar
io
1
o
u
tp
u
t
Fig
u
r
e
1
4
.
S
ce
n
ar
io
2
o
u
tp
u
t
B
y
s
t
u
d
y
i
n
g
t
h
e
f
ig
u
r
e,
o
n
e
c
an
o
b
s
er
v
e
t
h
e
d
i
f
f
er
en
ce
i
n
t
h
e
tr
aj
ec
to
r
y
f
o
r
m
at
io
n
a
s
d
is
p
lay
ed
i
n
Fig
u
r
e
1
3
.
I
t
is
b
en
e
f
icial
f
o
r
p
ick
in
g
u
p
an
d
p
lace
m
en
t
o
f
o
b
j
ec
ts
.
Fig
u
r
e
1
4
d
is
p
l
a
y
s
a
tr
aj
ec
to
r
y
w
h
ic
h
i
s
m
o
r
e
s
u
ited
f
o
r
w
eld
i
n
g
.
5.
CO
M
P
ARIS
O
N
T
ab
le
2
.
C
o
m
p
ar
is
o
n
b
et
w
ee
n
2
DOF
an
d
m
u
lti li
n
k
m
a
n
ip
u
l
ato
r
C
h
ar
ac
ter
is
tic
2
DOF
SC
AR
A
L
i
n
k
le
n
g
t
h
0
.
2
8
4
m
0
.
5
4
6
m
J
o
in
t v
alu
e
4
7
.
5
d
eg
5
8
d
eg
J
o
in
t v
elo
cit
y
9
5
d
eg
/s
ec
1
0
5
d
eg
/s
ec
J
o
in
t a
cc
eler
atio
n
1
5
0
d
eg
/s
ec
2
1
6
5
d
eg
/s
ec
2
Fo
r
ce
/ T
o
r
q
u
e
0
.
1
7
8
7
Nm
0
.
6
0
2
Nm
6.
F
UT
URE S
CO
P
E
T
h
is
m
et
h
o
d
o
lo
g
y
ca
n
b
e
u
s
e
d
f
o
r
g
en
er
ati
n
g
t
h
e
o
p
ti
m
a
l
p
ath
f
o
r
a
u
n
m
a
n
n
ed
au
to
n
o
m
o
u
s
v
eh
ic
le
o
r
a
air
cr
af
t c
o
n
tr
o
llin
g
i
n
th
e
m
ilit
ar
y
ap
p
licatio
n
s
.
RE
F
E
R
E
NC
E
S
[1
]
A
Ge
n
e
ra
l,
F
a
st,
a
n
d
Ro
b
u
st
Im
p
lem
e
n
tatio
n
o
f
th
e
T
im
e
-
Op
ti
m
a
l
P
a
t
h
P
a
ra
m
e
teriz
a
ti
o
n
A
lg
o
rit
h
m
Qu
a
n
g
-
Cu
o
n
g
P
h
a
m
.
IEE
E
tran
sa
c
ti
o
n
o
n
ro
b
o
ti
c
s,
v
o
l.
3
0
,
n
o
.
6
,
De
c
e
m
b
e
r
2
0
1
4
.
[2
]
Op
ti
m
a
l
p
a
th
p
lan
n
i
n
g
f
o
r
m
u
lt
i
a
rm
,
m
u
lt
il
in
k
ro
b
o
ti
c
m
a
n
ip
u
lat
o
r
b
y
Jo
se
p
h
A
.
Ca
sc
io
De
c
e
m
b
e
r
2
0
0
8
.
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
OOP
A
p
p
r
o
a
ch
f
o
r
Mu
lti De
g
r
ee
o
f F
r
ee
d
o
m
R
o
b
o
tic
A
r
m
b
a
s
ed
o
n
K
in
ema
tics
.
.
.
(
R
a
s
h
m
ir
a
n
ja
n
Da
s
)
291
[3
]
A
c
o
o
p
e
ra
ti
v
e
p
a
th
p
lan
n
in
g
a
lg
o
rit
h
m
f
o
r
m
u
lt
ip
le
m
o
b
il
e
ro
b
o
t
s
y
ste
m
in
a
d
y
n
a
m
ic
e
n
v
iro
n
m
e
n
t
re
g
u
lar.
W
e
n
tao
Yu
1
,
Ju
n
P
e
n
g
1
,
*
,
X
iao
y
o
n
g
Zh
a
n
g
1
a
n
d
Ku
o
-
c
h
i
L
in
2
.
In
tern
a
ti
o
n
a
l
J
o
u
r
n
a
l
o
f
A
d
v
a
n
c
e
d
Ro
b
o
ti
c
S
y
ste
m
s,
Ju
n
e
2
0
1
4
.
[4
]
Re
a
c
ti
v
e
P
a
th
P
lan
n
in
g
in
a
D
y
n
a
m
i
c
En
v
iro
n
m
e
n
t
F
e
th
i
Be
lk
h
o
u
c
h
e
,
m
e
m
b
e
r
IEE
E
t
ra
n
sa
c
ti
o
n
o
n
ro
b
o
ti
c
s,
v
o
l.
2
5
,
n
o
.
4
,
a
u
g
u
st
2
0
0
9
.
[5
]
V
isio
n
b
a
se
d
,
d
istri
b
u
te
d
c
o
n
tro
l
law
s
f
o
r
m
o
ti
o
n
c
o
rd
i
n
a
ti
o
n
o
f
n
o
n
h
o
lo
n
o
m
ic
ro
b
o
ts,
Nim
a
M
o
sh
tag
h
,
m
e
m
b
e
r
IEE
E,
Na
n
th
a
n
M
ic
h
a
e
l,
m
e
m
b
e
r
IEE
E,
A
li
jad
b
a
b
a
ie,
se
n
io
r
m
e
m
b
e
r
IEE
E
a
n
d
Ko
sta
s
Da
n
ii
li
d
s
IEE
E
tr
a
n
sc
a
ti
o
n
o
n
r
o
b
o
ti
c
s,
v
o
l.
2
5
,
n
o
.
4
,
a
u
g
u
st
2
0
0
9
.
[6
]
T
i
m
e
Op
ti
m
a
l
M
o
ti
o
n
P
lan
n
i
n
g
a
n
d
M
o
ti
o
n
Co
n
tr
o
l
f
o
r
In
d
u
strial
M
a
n
ip
u
lato
rs.
N
o
rw
e
g
i
a
n
u
n
iv
e
rsity
o
f
S
c
ien
c
e
&
T
e
c
h
n
o
lo
g
y
.
M
a
y
2
0
1
4
(m
a
ste
r
th
e
sis)
[7
]
F
ig
u
re
1
-
f
ro
m
th
e
a
u
to
m
o
ti
v
e
in
d
u
stry
w
o
rk
in
g
e
n
v
iro
n
m
e
n
t.
Evaluation Warning : The document was created with Spire.PDF for Python.