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
.
3
,
Sep
tem
b
er
2
0
1
6
,
p
p
.
213
~
222
I
SS
N:
2089
-
4856
213
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
A New
Approa
ch
to the
So
lution o
f
Ro
bo
t
K
in
e
m
a
tic
s Ba
sed o
n
Rela
tive Tra
nsfo
r
m
a
tion M
a
tr
ices
M
o
ha
m
m
a
d Re
za
E
l
ha
m
i
,
I
m
a
n Da
s
hti
De
p
a
rte
m
e
n
t
o
f
M
e
c
h
a
n
ica
l
En
g
in
e
e
rin
g
,
Im
a
m
Ho
se
in
Un
iv
e
rsit
y
,
Ira
n
Art
icle
I
nfo
AB
ST
RAC
T
A
r
ticle
his
to
r
y:
R
ec
eiv
ed
J
u
n
2
,
2
0
1
6
R
ev
i
s
ed
A
u
g
6
,
2
0
1
6
A
cc
ep
ted
A
u
g
20
,
2
0
1
6
In
a
n
a
ly
z
in
g
ro
b
o
t
m
a
n
ip
u
lato
r
k
in
e
m
a
ti
c
s,
w
e
n
e
e
d
to
d
e
sc
rib
e
re
lativ
e
m
o
v
e
m
e
n
t
o
f
a
d
jac
e
n
t
li
n
k
a
g
e
s
o
r
jo
in
ts
i
n
o
rd
e
r
t
o
o
b
tain
th
e
p
o
se
o
f
e
n
d
e
ffe
c
to
r
(b
o
th
p
o
sit
io
n
a
n
d
o
ri
e
n
tatio
n
)
i
n
re
f
e
re
n
c
e
c
o
o
rd
i
n
a
te
f
r
a
m
e
.
De
n
a
v
it
-
Ha
rten
b
e
rg
e
sta
b
li
sh
e
d
a
m
e
th
o
d
b
a
se
d
o
n
a
4
×
4
h
o
m
o
g
e
n
o
u
s
m
a
tri
x
so
c
a
ll
e
d
“
A
”
m
a
tri
x
.
T
h
is
m
e
th
o
d
u
se
d
b
y
m
o
st
o
f
th
e
a
u
th
o
rs
f
o
r
k
in
e
m
a
ti
c
s
a
n
d
d
y
n
a
m
i
c
a
n
a
l
y
sis
o
f
th
e
ro
b
o
t
m
a
n
ip
u
lat
o
rs.
A
lt
h
o
u
g
h
it
h
a
s
m
a
n
y
a
d
v
a
n
tag
e
s,
h
o
w
e
v
e
r,
f
i
n
d
i
n
g
th
e
e
lem
e
n
ts
o
f
th
is
m
a
tri
x
a
n
d
li
n
k
/j
o
i
n
t‟s
p
a
ra
m
e
ters
is
so
m
e
ti
m
e
s
c
o
m
p
li
c
a
ted
a
n
d
c
o
n
f
u
sin
g
.
By
c
o
n
sid
e
ri
n
g
th
e
se
d
if
f
icu
lt
ies
,
th
e
a
u
th
o
rs
p
r
o
p
o
se
d
a
n
e
w
a
p
p
ro
a
c
h
c
a
ll
e
d
„c
o
n
v
e
n
ien
t
a
p
p
ro
a
c
h
‟
th
a
t
is
d
e
v
e
lo
p
e
d
b
a
se
d
o
n
“
Re
lativ
e
T
ra
n
s
fo
rm
a
ti
o
n
s
P
ri
n
c
ip
le”
.
It
p
r
o
v
id
e
s
a
v
e
r
y
si
m
p
le
a
n
d
c
o
n
v
e
n
ien
t
w
a
y
f
o
r
th
e
so
lu
ti
o
n
o
f
ro
b
o
t
k
in
e
m
a
ti
c
s
c
o
m
p
a
re
d
to
th
e
c
o
n
v
e
n
ti
o
n
a
l
D
-
H
re
p
re
se
n
tatio
n
.
In
o
r
d
e
r
to
c
larify
th
is
p
o
in
t
,
t
h
e
k
in
e
m
a
ti
c
s
o
f
th
e
w
e
ll
-
k
n
o
w
n
S
tan
f
o
rd
m
a
n
ip
u
lato
r
h
a
s
b
e
e
n
so
lv
e
d
th
ro
u
g
h
D
-
H
re
p
re
se
n
tatio
n
a
s
w
e
ll
a
s
c
o
n
v
e
n
ien
t
a
p
p
ro
a
c
h
a
n
d
t
h
e
re
su
lt
s are
c
o
m
p
a
re
d
.
K
ey
w
o
r
d
:
C
o
n
v
en
ie
n
t
a
p
p
r
o
ac
h
Den
a
v
it
h
ar
te
n
b
er
g
R
elati
v
e
t
r
an
s
f
o
r
m
atio
n
P
r
in
cip
le
R
o
b
o
t
k
in
e
m
atic
s
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
:
Mo
h
a
m
m
ad
R
ez
a
E
l
h
a
m
i,
Dep
ar
te
m
en
t o
f
Me
ch
a
n
ical
E
n
g
i
n
ee
r
i
n
g
,
I
m
a
m
Ho
s
ein
U
n
i
v
er
s
it
y
,
B
ab
ae
i H
ig
h
w
a
y
,
T
eh
r
an
,
I
r
an
,
E
m
ail:
m
el
h
a
m
i@
ih
u
.
ac
.
ir
1.
I
NT
RO
D
UCT
I
O
N
Kin
e
m
atic
an
a
l
y
s
is
is
a
f
u
n
d
am
en
tal
p
ar
t
o
f
m
ec
h
a
n
is
m
d
es
ig
n
a
n
d
an
al
y
s
i
s
th
at
h
as
a
lo
n
g
h
is
to
r
y
o
f
estab
li
s
h
m
e
n
t.
R
eu
lea
u
x
[
1
]
d
ev
elo
p
ed
a
s
y
m
b
o
lic
m
e
t
h
o
d
o
f
d
escr
ib
in
g
th
e
k
i
n
e
m
atic
p
r
o
p
er
ties
o
f
a
m
ec
h
a
n
i
s
m
.
Den
a
v
it
an
d
H
ar
ten
b
er
g
[
2
]
s
u
b
s
eq
u
en
tl
y
o
b
tain
ed
a
n
e
w
f
o
r
m
o
f
k
in
e
m
atic
n
o
tatio
n
r
ep
r
esen
tin
g
all
p
o
s
s
ib
le
f
o
r
m
s
o
f
lo
w
er
-
p
air
m
ec
h
a
n
is
m
.
T
h
is
n
e
w
r
ep
r
esen
tat
io
n
e
x
p
r
ess
ed
in
ter
m
s
o
f
a
4
×
4
h
o
m
o
g
e
n
eo
u
s
tr
a
n
s
f
o
r
m
a
tio
n
m
atr
i
x
a
n
d
t
h
e
e
f
f
ec
t
o
f
a
n
u
m
b
er
o
f
co
u
p
led
lin
k
s
was
d
eter
m
i
n
ed
f
r
o
m
th
e
p
r
o
d
u
ct
o
f
a
n
u
m
b
er
o
f
m
atr
ices.
P
au
l
an
d
Sh
i
m
a
n
o
[
3
]
r
ec
o
g
n
ized
th
at
th
e
e
f
f
ec
t
o
f
a
s
in
g
le
j
o
in
t
an
d
its
a
s
s
o
ciate
d
li
n
k
o
n
a
r
o
b
o
t
co
u
ld
b
e
d
escr
ib
ed
b
y
f
o
u
r
p
ar
am
eter
s
a
n
d
o
b
tain
ed
a
s
i
n
g
le
h
o
m
o
g
en
o
u
s
m
atr
i
x
(
th
e
A
m
atr
i
x
)
r
ep
r
esen
ti
n
g
th
ese
p
ar
a
m
eter
s
.
Fo
r
w
ar
d
a
n
d
in
v
er
s
e
k
i
n
e
m
atic
s
o
lu
tio
n
s
d
er
i
v
ed
f
o
r
t
h
e
Stan
f
o
r
d
A
r
m
.
Su
b
s
eq
u
en
t
l
y
P
au
l,
Sh
i
m
an
o
a
n
d
Ma
y
o
r
[
4
]
illu
s
tr
ated
t
h
e
m
et
h
o
d
b
y
d
er
iv
i
n
g
s
o
lu
tio
n
s
f
o
r
P
u
m
a
m
a
n
ip
u
la
to
r
.
L
ee
[
5
]
a
n
d
F
u
,
Go
n
za
lez
a
n
d
L
ee
[
7
]
u
s
ed
t
h
e
s
i
m
ilar
m
et
h
o
d
to
d
e
ter
m
i
n
e
co
o
r
d
in
ate
tr
an
s
f
o
r
m
atio
n
s
f
o
r
a
P
u
m
a
r
o
b
o
t
to
p
er
f
o
r
m
k
i
n
e
m
a
tics
a
n
d
d
y
n
a
m
ic
s
an
al
y
s
is
.
T
h
is
tr
an
s
f
o
r
m
atio
n
m
eth
o
d
th
en
e
s
tab
lis
h
ed
as
a
s
ta
n
d
ar
d
to
o
l
f
o
r
all
au
th
o
r
s
in
r
o
b
o
t
k
in
e
m
atic
s
.
T
h
is
s
ta
n
d
ar
d
h
o
m
o
g
e
n
o
u
s
tr
an
s
f
o
r
m
atio
n
ap
p
r
o
ac
h
h
as
estab
lis
h
ed
f
o
u
r
p
ar
a
m
eter
s
d
escr
ib
in
g
ea
ch
j
o
in
t
a
n
d
t
h
e
ass
o
ciate
d
li
n
k
I
a
s
f
o
llo
w
s
: t
h
e
j
o
in
t a
n
g
le
i
,
th
e
lin
k
len
g
t
h
i
a
,
th
e
lin
k
o
f
f
s
et
i
d
,
th
e
j
o
in
t t
w
i
s
t
i
Dete
r
m
i
n
atio
n
o
f
th
e
s
e
p
ar
a
m
eter
s
co
u
ld
b
e
d
i
f
f
icu
l
t,
p
ar
ticu
lar
l
y
f
o
r
o
n
es
w
h
o
u
n
f
a
m
iliar
w
it
h
r
o
b
o
tics
,
th
is
p
r
o
b
lem
b
ec
o
m
e
s
w
o
r
s
e
in
d
eter
m
i
n
i
n
g
th
e
j
o
i
n
t
t
w
is
t
w
h
ile
o
n
e
o
r
m
o
r
e
lin
k
len
g
t
h
s
ar
e
ze
r
o
.
T
h
is
p
ap
er
is
i
n
ten
d
ed
to
s
u
g
g
es
t
a
s
i
m
p
l
if
ied
m
et
h
o
d
to
o
b
tain
t
h
e
A
m
atr
ices
b
ased
o
n
“
R
ela
tiv
e
T
r
an
s
f
o
r
m
atio
n
s
P
r
in
cip
le”.
T
h
e
ap
p
licatio
n
o
f
th
i
s
co
n
v
en
ien
t
ap
p
r
o
ac
h
is
ill
u
s
tr
ated
b
y
p
er
f
o
r
m
i
n
g
th
e
f
o
r
w
ar
d
k
i
n
e
m
atic
a
n
al
y
s
is
o
n
Sta
n
f
o
r
d
m
a
n
ip
u
la
to
r
s
to
o
b
tain
th
e
e
n
d
ef
f
ec
to
r
p
o
s
itio
n
an
d
o
r
ien
tatio
n
v
ec
to
r
s
.
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
.
3
,
Sep
tem
b
er
2
0
1
6
:
2
1
3
–
2
2
2
214
2.
DE
NAV
I
T
-
H
AR
T
E
NB
E
R
G
M
E
T
H
O
D
E
x
p
lain
i
n
g
r
esear
ch
c
h
r
o
n
o
lo
g
ical,
in
c
lu
d
i
n
g
r
esear
c
h
d
esi
g
n
,
r
esear
c
h
p
r
o
ce
d
u
r
e
(
in
th
e
f
o
r
m
o
f
alg
o
r
ith
m
s
,
P
s
e
u
d
o
co
d
e
o
r
o
th
er
)
,
h
o
w
to
test
a
n
d
d
ata
ac
q
u
is
i
tio
n
[
1
]
-
[
3
]
.
T
h
e
d
escr
ip
ti
o
n
o
f
t
h
e
co
u
r
s
e
o
f
r
esear
ch
s
h
o
u
ld
b
e
s
u
p
p
o
r
ted
r
ef
er
en
ce
s
,
s
o
th
e
ex
p
la
n
atio
n
ca
n
b
e
ac
ce
p
ted
s
cien
ti
f
icall
y
[
2
]
,
[
4
]
.
Den
a
v
it
-
Har
ten
b
er
g
m
et
h
o
d
r
ep
r
esen
ts
ea
c
h
tr
an
s
f
o
r
m
atio
n
b
y
a
s
p
ec
i
f
ic
co
n
v
e
n
tio
n
e
s
ta
b
lis
h
ed
b
y
a
s
er
ies
o
f
d
ef
in
it
io
n
s
.
C
o
n
s
id
er
a
r
o
b
o
t
m
a
n
ip
u
la
to
r
w
it
h
n
k
in
e
m
at
ic
p
air
s
(
r
o
tated
an
d
p
r
is
m
atic)
.
L
et
L
i
t
h
e
i
-
t
h
li
n
k
a
n
d
j
i
th
e
i
-
t
h
k
in
e
m
a
tic
p
air
b
et
w
ee
n
L
i
-
1
an
d
L
i
,
i
=
1
,
2
,
…,
n
.
L
0
is
th
e
lin
k
b
e
t
w
ee
n
b
ase
an
d
t
h
e
f
ir
s
t
k
i
n
e
m
a
tic
p
air
.
Af
ter
d
ef
in
i
n
g
a
r
ef
er
e
n
ce
co
o
r
d
in
ate
s
y
s
te
m
,
o
n
e
co
o
r
d
in
ate
s
y
s
te
m
ass
i
g
n
ed
f
o
r
ea
ch
j
o
in
t
o
f
r
o
b
o
t
m
a
n
ip
u
la
to
r
,
also
in
th
e
e
n
d
-
e
f
f
ec
to
r
,
in
o
r
d
er
to
estab
lis
h
t
h
e
co
o
r
d
in
ate
tr
an
s
f
o
r
m
atio
n
b
et
w
ee
n
li
n
k
s
,
a
n
d
s
o
lv
e
th
e
r
o
b
o
tic
k
in
e
m
atic
s
.
I
n
s
u
m
m
ar
y
,
De
n
av
it
-
Har
ten
b
er
g
m
eth
o
d
d
ef
in
e
s
a
f
r
a
m
e
F
i
,
i =
0
,
…
,
n
,
b
y
:
z
i
-
ax
is
: a
x
is
o
f
th
e
i +
1
j
o
in
t;
x
i
-
a
x
i
s
: is p
ar
allel
to
th
e
co
m
m
o
n
n
o
r
m
al:
1
i
i
i
x
z
z
;
y
i
-
a
x
i
s
: f
o
llo
w
s
f
r
o
m
r
i
g
h
t
-
h
a
n
d
r
u
le;
Oi
: in
ter
s
ec
tio
n
b
et
w
ee
n
z
i
ax
i
s
an
d
co
m
m
o
n
n
o
r
m
al;
'
i
O
: in
ter
s
ec
tio
n
b
et
w
ee
n
z
i
-
1
ax
is
an
d
co
m
m
o
n
n
o
r
m
al;
T
h
en
,
a
tr
an
s
f
o
r
m
atio
n
f
r
o
m
f
r
a
m
e
F
i
to
f
r
a
m
e
F
i
-
1
d
ef
i
n
ed
b
y
DH
p
ar
am
eter
s
:
a
i
: d
is
tan
ce
f
r
o
m
Oi
a
n
d
'
i
O
m
ea
s
u
r
ed
alo
n
g
co
m
m
o
n
n
o
r
m
al
;
d
i
: d
is
tan
ce
f
r
o
m
O
i
-
1
an
d
'
i
O
,
m
e
asu
r
ed
alo
n
g
z
i
;
i
:
an
g
le
b
et
w
ee
n
a
x
es
z
i
-
1
an
d
z
i
ab
o
u
t
ax
is
x
i
to
b
e
tak
en
p
o
s
itiv
e
w
h
en
r
o
tatio
n
i
s
m
a
d
e
co
u
n
ter
-
clo
ck
w
i
s
e;
i
:an
g
le
b
et
w
ee
n
a
x
es
x
i
-
1
an
d
x
i
ab
o
u
t
ax
is
z
i
-
1
to
b
e
ta
k
en
p
o
s
itiv
e
w
h
e
n
r
o
tatio
n
i
s
m
ad
e
co
u
n
ter
-
clo
ck
w
i
s
e;
Fig
u
r
e
1
.
D
-
H
p
ar
a
m
eter
s
d
e
f
i
n
itio
n
Mo
r
e
ab
o
u
t D
en
av
it Ha
r
te
n
b
er
g
co
n
v
e
n
tio
n
ca
n
b
e
f
o
u
n
d
in
[
6
,
1
2
,
1
4
]
.
I
n
g
en
er
al,
D
-
H
p
ar
a
m
eter
s
ca
n
b
e
tab
u
lated
,
leav
in
g
th
e
tr
a
n
s
f
o
r
m
atio
n
s
as
f
u
n
ctio
n
s
o
f
t
h
e
v
ar
iab
le
,
in
t
h
e
r
ev
o
l
u
te
ca
s
e,
o
r
d
,
in
th
e
p
r
is
m
atic
ca
s
e.
Ho
m
o
g
en
e
o
u
s
co
o
r
d
in
ates
allo
w
e
s
tab
lis
h
in
g
t
h
e
r
elatio
n
b
et
w
ee
n
t
w
o
ad
j
ac
en
t li
n
k
s
,
c
o
n
n
ec
ted
b
y
a
k
i
n
e
m
atic
p
air
,
f
o
llo
w
in
g
f
o
u
r
s
tep
s
:
R
o
ta
te
in
x
O
Tr
a
n
s
la
te
a
b
y
x
O
R
o
ta
te
in
z
O
Tr
a
n
s
la
te
d
in
z
O
T
h
e
s
tep
s
ab
o
v
e
ex
p
r
ess
th
e
p
r
o
d
u
ct
o
f
f
o
u
r
h
o
m
o
g
e
n
eo
u
s
tr
an
s
f
o
r
m
atio
n
m
a
tr
ix
f
r
o
m
f
r
am
e
F
i
to
f
r
a
m
e
F
i
-
1
,
i =
1
,
…,
n
,
th
at
is
,
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
A
N
ew A
p
p
r
o
a
ch
to
th
e
S
o
lu
ti
o
n
o
f R
o
b
o
t Ki
n
ema
tic
B
a
s
ed
o
n
R
ela
tive
.
.
.
(
Mo
h
a
mma
d
R
e
z
a
E
lh
a
mi
)
215
1
(
)
(
)
(
)
(
)
c
o
s
c
o
s
sin
sin
sin
c
o
s
sin
c
o
s
c
o
s
sin
c
o
s
sin
0
sin
c
o
s
0
0
0
1
i
i
z
i
z
i
x
i
x
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
A
T
d
R
T
a
R
a
a
d
(
1
)
3.
T
H
E
CO
NV
E
NI
E
N
T
AP
P
R
O
ACH
3
.
1
.
Co
o
rdina
t
e
F
ra
m
e
s
A
h
o
m
o
g
e
n
o
u
s
tr
an
s
f
o
r
m
atio
n
m
atr
i
x
ca
n
b
e
u
s
ed
to
d
es
cr
ib
e
()
o
x
y
z
f
r
a
m
e
w
i
th
r
esp
ec
t
to
a
r
ef
er
en
ce
co
o
r
d
in
ate
f
r
a
m
e
()
o
x
y
z
.
T
h
e
f
ir
s
t
th
r
ee
r
o
w
s
o
f
th
i
s
4
*
4
m
atr
i
x
ar
e
co
m
p
o
s
ed
o
f
a
3
*
1
O
v
ec
to
r
(
d
en
o
tin
g
t
h
e
p
o
s
itio
n
o
f
()
o
x
y
z
o
r
ig
in
i
n
()
o
x
y
z
f
r
a
m
e)
an
d
a
3
*
3
r
o
tatio
n
R
m
atr
ix
(
d
en
o
ti
n
g
th
e
o
r
ien
tatio
n
o
f
()
o
x
y
z
ax
es i
n
()
o
x
y
z
f
r
a
m
e)
.
''
'
3
*
3
3
*
1
0
0
0
1
O
x
y
z
O
x
y
z
O
x
y
z
O
x
y
z
O
x
y
z
O
x
y
z
RO
T
(
2
)
T
h
is
is
ill
u
s
tr
ated
th
r
o
u
g
h
an
e
x
a
m
p
le.
C
o
n
s
id
er
a
co
o
r
d
in
ate
f
r
a
m
e,
i.e
.
'
()
o
x
y
z
,
s
p
ec
if
ied
b
y
:
'
0
0
1
1
1
0
0
3
0
1
0
2
0
0
0
1
Oxyz
Oxyz
T
(
3
)
T
h
e
r
o
tatio
n
an
d
p
o
s
itio
n
m
atr
ices a
r
e
as f
o
llo
w
s
:
0
0
1
1
0
0
'
0
1
0
O
x
y
z
R
O
x
y
z
(
4
)
'
1
3
2
O
x
y
z
O
x
y
z
O
(
5
)
T
h
u
s
,
th
e
p
o
s
itio
n
an
d
d
ir
ec
tio
n
o
f
()
o
x
y
z
f
r
a
m
e
w
it
h
r
esp
ec
t to
()
o
x
y
z
f
r
a
m
e
ar
e
ex
p
r
ess
ed
as:
'
'
'
'
0
1
0
0
0
0
1
0
1
0
0
0
1
3
2
1
T
T
T
T
i
j
k
o
(
6
)
An
d
t
h
er
ef
o
r
e,
t
h
e
o
r
ig
i
n
o
f
()
O
X
Y
Z
i
s
lo
ca
ted
o
n
th
e
p
o
in
t
o
f
(
-
1
,
3
,
2
)
an
d
d
ir
ec
tio
n
o
f
,,
X
Y
Z
ax
es
ar
e
alo
n
g
Y,
-
Z,
-
X
d
ir
ec
tio
n
o
f
o
r
ig
i
n
al
f
r
a
m
e,
r
esp
ec
tiv
el
y
.
C
o
n
s
eq
u
e
n
tl
y
,
()
O
X
Y
Z
f
r
a
m
e
co
u
ld
b
e
o
u
tlin
ed
i
n
a
v
er
y
s
i
m
p
le
m
a
n
n
er
.
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
.
3
,
Sep
tem
b
er
2
0
1
6
:
2
1
3
–
2
2
2
216
3
.
2
.
G
eo
m
et
ric
I
nte
rpre
t
a
t
io
n o
f
ho
m
o
g
eno
us
T
ra
n
s
f
o
rm
a
t
io
n M
a
t
rix
(
H
T
M
)
,
T
:
Descr
ib
in
g
g
eo
m
e
tr
ic
in
ter
p
r
etatio
n
o
f
Ho
m
o
g
en
o
u
s
tr
an
s
f
o
r
m
at
io
n
m
a
tr
ix
,
T
,
let‟
s
h
a
v
e
a
lo
o
k
at
p
r
ec
ed
in
g
e
x
a
m
p
le
f
r
o
m
a
n
o
th
er
an
g
le.
C
o
n
s
id
er
th
e
w
a
y
s
wh
ich
()
O
X
Y
Z
f
r
a
m
e
i
s
r
ea
c
h
ed
to
it
s
c
u
r
r
en
t s
tate
f
r
o
m
i
ts
i
n
it
ial
s
tate
w
it
h
r
e
s
p
ec
t
to
th
e
b
ase
f
r
a
m
e
(
OX
Y
Z)
.
T
h
er
e
ar
e
t
w
o
w
a
y
s
f
o
r
t
h
e
s
o
l
u
tio
n
o
f
th
i
s
p
r
o
b
lem
:
w
e
ca
n
ei
th
er
f
ir
s
t
tr
an
s
late
th
e
o
r
ig
i
n
o
f
()
O
X
Y
Z
f
r
a
m
e
to
th
e
d
esire
d
p
o
s
itio
n
i.e
.
1
3
2
1
T
,
th
en
r
o
tate
it
ab
o
u
t
Y
a
n
d
Z
ax
es
o
f
c
u
r
r
en
t
f
r
a
m
e
r
esp
ec
ti
v
el
y
;
Or
f
ir
s
t
r
o
tate
i
t
ab
o
u
t
Y
a
n
d
X
ax
e
s
o
f
b
ase
f
r
a
m
e
r
esp
ec
ti
v
el
y
,
th
e
n
tr
an
s
l
ate
it to
th
e
d
esire
d
p
o
s
itio
n
,
i.e
.
1
3
2
1
T
.
Sin
ce
t
h
ese
t
w
o
w
a
y
s
lead
to
d
if
f
er
en
t
o
r
d
er
o
f
m
atr
i
x
m
u
ltip
licatio
n
an
d
m
atr
i
x
m
u
ltip
licatio
n
is
n
o
n
-
co
m
m
u
tati
v
e,
t
h
er
e
co
m
e
s
a
m
ain
p
r
o
b
lem
th
at
in
w
h
a
t
o
r
d
er
th
ese
m
atr
ice
s
ar
e
m
u
l
ti
p
lied
in
o
r
d
er
to
g
et
th
e
s
a
m
e
r
es
u
lt.
3.
3
.
Rela
t
iv
e
T
ra
ns
f
o
r
m
a
t
io
ns
P
rinci
ple (
RT
P
)
T
h
er
e
is
a
f
u
n
d
a
m
en
ta
l
r
u
l
e
b
eh
i
n
d
th
e
o
r
d
er
o
f
m
u
l
tip
licatio
n
,
w
h
ic
h
i
s
ca
lled
“Relati
v
e
T
r
an
s
f
o
r
m
atio
n
P
r
in
cip
le”
[
8
]
.
T
h
is
p
r
in
cip
le
s
tates
w
h
er
e
we
ca
n
m
u
ltip
l
y
th
e
m
atr
i
ce
s
in
th
e
o
r
d
er
o
f
tr
an
s
f
o
r
m
atio
n
o
p
er
atio
n
o
r
d
o
it in
th
e
r
e
v
er
s
e
d
ir
ec
tio
n
.
Mu
ltip
licat
io
n
o
f
t
h
e
m
atr
ice
s
in
t
h
e
o
r
d
er
o
f
tr
a
n
s
f
o
r
m
atio
n
o
p
er
atio
n
s
o
ca
lled
P
o
s
t
-
m
u
lt
ip
licatio
n
,
m
ea
n
s
t
h
at
tr
a
n
s
f
o
r
m
atio
n
r
ef
er
s
to
th
e
C
u
r
r
en
t
C
o
o
r
d
in
ate
Fra
m
e;
a
n
d
m
u
ltip
lica
tio
n
i
n
t
h
e
r
ev
er
s
e
o
r
d
er
o
f
o
p
er
atio
n
s
is
ca
lled
Pre
-
mu
lt
ip
licatio
n
an
d
m
ea
n
s
th
a
t
tr
an
s
f
o
r
m
atio
n
r
ef
er
r
ed
w
.
r
.
t.
B
ase
o
r
R
ef
er
en
ce
C
o
o
r
d
in
ate
Fra
m
e.
T
h
er
ef
o
r
e,
th
er
e
i
s
a
“
P
r
e
-
B
ase;
P
o
s
t
-
C
u
r
r
en
t”
r
eso
l
u
tio
n
f
o
r
t
h
i
s
p
r
in
ci
p
le.
T
o
clar
if
y
t
h
i
s
p
o
in
t,
let‟
s
h
a
v
e
a
lo
o
k
at
th
e
p
r
ev
io
u
s
ex
a
m
p
le
a
g
ain
.
T
h
er
e
ar
e
t
w
o
ap
p
r
o
ac
h
es
f
o
r
f
r
am
e
(
XYZ
)
o
r
B
ase
-
Fra
m
e
to
r
ea
ch
f
r
a
m
e
(
)
'
X
Y
Z
o
r
C
u
r
r
en
t Fr
a
m
e
as s
h
o
w
n
i
n
f
ig
u
r
e
2
.
P
o
s
t
-
Curre
nt
(
1
,
3
,
2)
(
,
90
)
(
,
90
)
1
0
0
1
0
0
1
0
0
1
0
0
0
1
0
3
0
1
0
0
1
0
0
0
0
0
1
2
1
0
0
0
0
0
1
0
0
0
0
1
0
0
0
1
0
0
0
1
0
0
1
1
1
0
0
3
0
1
0
2
0
0
0
1
T
rans
R
ot
Y
R
ot
Z
(
7
)
P
re
-
B
a
s
e
(
1
,
3
,
2)
(
,
90
)
(
,
90
)
1
0
0
1
1
0
0
0
0
0
1
0
0
1
0
3
0
0
1
0
0
1
0
0
0
0
1
2
0
1
0
0
1
0
0
0
0
0
0
1
0
0
0
1
0
0
0
1
0
0
1
1
1
0
0
3
0
1
0
2
0
0
0
1
T
rans
R
ot
X
R
ot
Y
(
8
)
I
t
is
s
h
o
w
n
t
h
at
alth
o
u
g
h
th
e
r
e
ar
e
tw
o
d
if
f
er
e
n
t
ap
p
r
o
ac
h
es
an
d
ev
en
t
u
all
y
t
w
o
d
i
f
f
er
e
n
t
m
atr
i
x
eq
u
atio
n
s
,
h
o
w
ev
er
,
th
e
r
es
u
lt
s
ar
e
th
e
s
a
m
e.
(
See
Fig
u
r
e
2
)
.
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
A
N
ew A
p
p
r
o
a
ch
to
th
e
S
o
lu
ti
o
n
o
f R
o
b
o
t Ki
n
ema
tic
B
a
s
ed
o
n
R
ela
tive
.
.
.
(
Mo
h
a
mma
d
R
e
z
a
E
lh
a
mi
)
217
Fig
u
r
e
2
.
R
elativ
e
T
r
an
s
f
o
r
m
a
tio
n
P
r
in
cip
le
3.
4
.
T
he
Co
nv
enient
Appro
a
ch
I
n
a
n
al
y
s
is
o
f
r
o
b
o
t
k
i
n
e
m
a
tic
s
,
ev
er
y
l
in
k
/j
o
in
t
p
air
a
ttrib
u
t
ed
w
i
th
a
1
i
i
A
m
a
tr
ix
,
w
h
ich
d
esc
r
ib
es
its
p
o
s
e
(
b
o
th
p
o
s
itio
n
an
d
d
ir
ec
tio
n
)
r
elativ
e
to
th
e
p
r
ev
io
u
s
o
n
e.
I
n
th
e
p
r
ev
io
u
s
s
ec
tio
n
,
th
e
“
P
r
e
-
B
ase
P
o
s
t
-
C
u
r
r
en
t”
r
eso
lu
tio
n
in
tr
o
d
u
ce
d
.
T
o
d
e
r
iv
e
a
1
i
i
A
m
atr
ix
t
h
er
e
ar
e
o
n
l
y
t
w
o
r
u
le
s
to
b
e
ap
p
lied
:
1)
T
h
e
i
Z
ax
is
lies
alo
n
g
t
h
e
ax
i
s
o
f
jo
in
t
i‟
s
m
o
v
e
m
e
n
t,
i
X
an
d
i
Y
ax
es
d
ir
ec
tio
n
s
ar
e
o
p
tio
n
al,
j
u
s
t
s
atis
f
y
t
h
e
r
ig
h
t
-
h
an
d
r
u
les
2)
B
y
u
s
i
n
g
r
elati
v
e
tr
an
s
f
o
r
m
atio
n
s
p
r
i
n
cip
le,
tr
an
s
f
o
r
m
a
tio
n
m
atr
ix
o
f
l
in
k
/j
o
in
t
i
w
it
h
r
esp
ec
t
to
j
o
in
t
i
-
1
,
w
h
ic
h
is
co
m
p
o
s
ed
o
f
f
i
x
ed
an
d
v
ar
iab
le
k
i
n
e
m
atic
p
ar
a
m
eter
s
,
co
u
ld
b
e
o
b
tain
ed
d
ir
ec
tl
y
an
d
v
er
y
s
tr
aig
h
t
f
o
r
w
ar
d
.
I
n
f
ac
t,
ev
er
y
A
m
atr
ix
is
p
r
o
d
u
ce
d
b
y
m
u
ltip
licatio
n
o
f
t
wo
m
atr
ices.
On
e
is
a
f
ix
ed
HT
M
m
atr
i
x
an
d
t
h
e
o
th
er
is
a
v
ar
iab
le
HT
M
m
atr
i
x
.
T
h
e
f
i
x
ed
HT
M
m
a
tr
ix
d
escr
ib
es
th
e
o
r
ig
i
n
al
g
eo
m
etr
ic
s
tr
u
ct
u
r
e
o
f
th
e
l
in
k
a
n
d
i
s
d
er
iv
ed
b
y
s
u
b
s
titu
ti
n
g
t
h
e
r
elat
iv
e
d
ir
ec
tio
n
o
f
X,
Y
a
n
d
Z
a
x
es
an
d
r
elat
iv
e
p
o
s
itio
n
o
f
j
o
in
t
i
o
r
ig
in
f
r
a
m
e
‟
s
(
P
v
ec
to
r
)
in
to
i
-
1
co
o
r
d
in
ate
f
r
a
m
e.
J
u
s
t
lik
e
eq
u
atio
n
s
(
3
)
-
(
6
)
in
th
e
p
r
ev
io
u
s
s
ec
tio
n
ex
a
m
p
le,
w
e
ca
n
d
er
iv
e
th
e
m
i
n
s
ta
n
tl
y
an
d
p
u
t t
h
e
m
i
n
th
e
s
a
m
e
o
r
d
er
o
f
co
lu
m
n
s
v
er
y
s
i
m
p
le.
An
o
th
er
o
n
e,
th
e
v
ar
iab
le
HT
M
m
atr
ix
is
a
f
u
n
ctio
n
o
f
j
o
in
t
v
ar
iab
le
p
ar
am
eter
an
d
ch
ar
a
cter
izes
th
e
r
elativ
e
m
o
v
e
m
e
n
t
allo
w
ed
at
ea
ch
j
o
in
t
an
d
is
o
n
ly
a
r
o
tatio
n
o
r
tr
an
s
latio
n
ab
o
u
t/alo
n
g
Z
ax
is
t
h
at
co
v
er
s
i
an
d
i
d
p
ar
a
m
eter
s
i
n
r
e
v
o
lu
te
a
n
d
p
r
is
m
atic
j
o
in
ts
,
r
esp
ec
tiv
e
l
y
.
T
h
e
tr
an
s
f
o
r
m
atio
n
m
atr
ix
f
o
r
ea
ch
j
o
in
t
is
th
e
n
o
b
tain
ed
b
y
m
u
l
tip
licat
io
n
o
f
th
e
s
e
t
w
o
m
atr
ice
s
,
w
h
i
ch
in
p
o
s
t
-
cu
r
r
e
n
t a
p
p
r
o
ac
h
is
as:
*
A
F
V
(
9
)
an
d
in
p
r
e
-
b
ase
ap
p
r
o
ac
h
is
:
*
A
V
F
(
10)
C
o
n
s
id
er
in
g
F
a
n
d
V
as
f
i
x
ed
an
d
v
ar
iab
le
HT
M
o
f
ea
ch
j
o
in
t
r
esp
ec
ti
v
el
y
.
I
t
i
s
w
o
r
t
h
to
n
o
tice
t
h
at
f
o
r
co
n
v
e
n
ie
n
ce
u
s
u
all
y
“
P
o
s
t
-
c
u
r
r
en
t”
p
ar
t
o
f
t
h
e
r
eso
l
u
tio
n
i
s
u
s
ed
to
d
er
iv
e
th
e
A
m
atr
i
x
f
o
r
en
tire
k
in
e
m
at
ic
eq
u
atio
n
s
f
r
o
m
b
ase
f
r
a
m
e
to
en
d
e
f
f
ec
to
r
to
d
escr
ib
e
th
e
f
o
r
w
ar
d
k
in
e
m
at
ic
o
f
r
o
b
o
t a
r
m
s
.
4.
CO
M
P
ARIS
O
N
O
F
T
H
E
T
WO
M
E
T
H
O
DS:
D
-
H
NO
T
AT
I
O
N
AN
D
CO
NVE
NI
E
N
T
AP
P
RO
ACH
D
-
H
n
o
tatio
n
h
as
estab
li
s
h
ed
a
v
er
y
ef
f
icie
n
t
an
d
ex
te
n
s
i
v
e
m
et
h
o
d
to
d
escr
ib
e
th
e
HT
M
o
f
ev
er
y
lin
k
in
an
y
m
a
n
ip
u
lato
r
.
I
t
i
s
also
a
co
n
cise
m
o
d
el
o
f
ev
er
y
k
in
d
o
f
lin
k
/j
o
in
t
p
air
in
an
y
m
ec
h
an
i
s
m
.
A
lt
h
o
u
g
h
it
h
a
s
s
o
m
e
ad
v
a
n
ta
g
es
s
o
th
a
t
h
a
s
b
ec
o
m
e
th
e
m
o
s
t
p
o
p
u
lar
r
o
b
o
t
k
in
e
m
atics
an
al
y
s
is
m
eth
o
d
i
n
liter
atu
r
e,
th
er
e
a
r
e
m
a
n
y
d
i
f
f
i
cu
ltie
s
an
d
co
m
p
licatio
n
s
i
n
th
e
p
r
o
ce
s
s
o
f
p
ar
am
eter
s
d
eter
m
i
n
atio
n
.
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
.
3
,
Sep
tem
b
er
2
0
1
6
:
2
1
3
–
2
2
2
218
T
h
e
m
ai
n
d
if
f
ic
u
ltie
s
ar
is
es
i
n
r
ec
o
g
n
itio
n
o
f
t
h
e
d
if
f
er
en
ce
b
et
w
ee
n
i
d
an
d
i
a
or
i
an
d
i
as
w
ell
as
a
m
b
i
g
u
it
y
in
th
e
d
ef
i
n
itio
n
o
f
th
e
s
i
g
n
o
f
i
,
esp
ec
iall
y
w
h
e
n
r
o
b
o
t
h
as
d
if
f
er
e
n
t
co
n
f
i
g
u
r
atio
n
s
w
h
ic
h
m
ak
e
s
t
h
e
s
u
b
s
tit
u
tio
n
o
f
p
ar
a
m
eter
s
i
n
A
m
atr
i
x
a
ti
m
e
-
c
o
n
s
u
m
i
n
g
ca
lc
u
latio
n
.
T
h
ese
d
is
ad
v
a
n
ta
g
es
ca
u
s
e
m
aj
o
r
p
r
o
b
lem
s
f
o
r
d
ev
elo
p
m
en
t
o
f
r
o
b
o
t
d
y
n
a
m
ic
co
n
tr
o
l
s
y
s
te
m
i
n
i
n
d
u
s
tr
y
an
d
p
o
p
u
lar
it
y
o
f
r
o
b
o
tics
i
n
ed
u
ca
tio
n
s
y
s
te
m
,
m
ai
n
l
y
f
o
r
th
e
s
t
u
d
en
t
s
w
h
o
ar
e
in
t
h
e
f
ir
s
t
s
tag
e
s
o
f
lear
n
i
n
g
r
o
b
o
tics
.
T
h
e
C
o
n
v
e
n
ien
t
A
p
p
r
o
ac
h
,
w
h
ic
h
is
p
r
o
p
o
s
ed
in
th
is
p
ap
er
h
as
co
n
s
id
er
ed
th
ese
p
r
o
b
le
m
s
an
d
o
f
f
er
ed
a
s
i
m
p
lifie
d
m
e
th
o
d
t
o
o
b
tain
t
h
e
A
m
atr
i
x
to
d
eter
m
in
e
k
in
e
m
atic
co
n
f
i
g
u
r
atio
n
o
f
ea
ch
li
n
k
/j
o
in
t
p
air
o
f
r
o
b
o
t
ar
m
s
.
T
h
is
m
et
h
o
d
is
s
tr
aig
h
t
f
o
r
w
ar
d
,
h
as
m
o
r
e
c
lar
it
y
in
co
n
ce
p
t
a
n
d
less
co
m
p
lex
i
t
y
in
c
o
m
p
u
tatio
n
;
t
h
er
ef
o
r
e
it
h
a
s
ea
s
y
i
m
p
le
m
e
n
tatio
n
in
k
i
n
e
m
atic
co
n
tr
o
l
an
d
ea
s
y
u
n
d
er
s
ta
n
d
in
g
f
o
r
ed
u
ca
tio
n
o
f
r
o
b
o
tics
.
T
o
g
et
f
a
m
i
liar
w
ith
t
h
is
m
e
th
o
d
,
it
is
o
n
l
y
r
eq
u
ir
ed
to
u
n
d
er
s
tan
d
th
e
co
r
e
co
n
ce
p
t
o
f
R
elati
v
e
T
r
an
s
f
o
r
m
atio
n
s
P
r
in
cip
le
(
R
T
P
)
an
d
its
P
r
e
-
C
u
r
r
en
t
P
o
s
t
-
B
ase
r
eso
lu
tio
n
to
d
er
iv
e
A
m
atr
ix
v
er
y
ea
s
i
l
y
an
d
d
ir
ec
tl
y
.
T
h
er
e
is
n
o
m
o
r
e
an
x
io
u
s
ab
o
u
t
id
en
t
if
ica
tio
n
a
n
d
d
eter
m
in
at
io
n
o
f
t
h
o
s
e
f
o
u
r
li
n
k
/j
o
in
t
p
ar
a
m
e
ter
s
.
Fo
r
co
m
p
u
tatio
n
m
atter
t
h
er
e
is
m
er
el
y
th
e
n
ee
d
to
k
n
o
w
t
h
e
r
o
tatio
n
/tra
n
s
la
tio
n
m
atr
ice
s
ab
o
u
t
Z
-
a
x
is
a
n
d
m
atr
i
x
m
u
ltip
licat
io
n
r
u
le
s
.
T
h
e
m
ai
n
p
r
iv
i
leg
e
o
f
t
h
i
s
m
eth
o
d
ca
n
b
e
s
h
o
w
n
in
m
o
s
t
r
o
b
o
ts
,
esp
ec
ially
i
n
d
u
s
tr
ial
r
o
b
o
ts
f
o
r
co
n
v
e
n
ien
ce
i
n
a
n
al
y
s
is
,
esp
e
ciall
y
d
y
n
a
m
ic
an
a
l
y
s
is
,
a
n
d
ev
en
t
u
all
y
s
i
m
p
licit
y
i
n
m
a
n
u
f
ac
t
u
r
in
g
,
t
h
e
y
ar
e
d
esig
n
ed
w
it
h
0
i
o
r
i
n
r
o
b
o
t
ar
m
s
.
T
h
er
e
f
o
r
e,
d
er
iv
atio
n
o
f
f
ix
ed
HT
M
o
f
A
m
atr
ix
b
ec
o
m
es
v
er
y
h
an
d
y
a
n
d
s
tr
aig
h
t
f
o
r
w
ar
d
.
H
o
w
e
v
er
,
f
o
r
i
in
an
y
co
n
d
itio
n
,
th
an
k
s
to
th
e
co
n
c
ep
t
clea
r
n
es
s
an
d
alg
o
r
ith
m
s
i
m
p
lic
it
y
,
t
h
is
ap
p
r
o
ac
h
is
s
til
l a
p
p
licab
le
co
m
p
ar
ed
to
D
-
H
n
o
tatio
n
.
5.
T
H
E
I
L
L
US
T
RA
T
I
O
N
M
E
T
H
O
D
T
o
s
h
o
w
th
a
t
th
e
co
n
v
e
n
ie
n
t
ap
p
r
o
ac
h
is
ap
p
licab
le
to
th
e
r
ea
l
an
d
s
tan
d
ar
d
in
d
u
s
tr
ial
r
o
b
o
ts
,
th
e
i
m
p
le
m
en
ta
tio
n
o
f
t
h
is
m
eth
o
d
is
ill
u
s
tr
ated
b
y
a
n
al
y
zin
g
t
h
e
f
o
r
w
ar
d
k
i
n
e
m
at
ic
to
o
b
ta
in
t
h
e
e
n
d
e
f
f
ec
to
r
p
o
s
e
v
ec
to
r
f
o
r
Stan
f
o
r
d
m
an
i
p
u
lato
r
.
T
h
is
m
a
n
ip
u
la
to
r
is
an
ex
a
m
p
le
o
f
a
s
p
h
er
ical
(
R
R
P
)
m
an
ip
u
lato
r
w
i
t
h
a
s
p
h
er
ical
w
r
i
s
t.
I
t
h
a
s
a
n
o
f
f
s
et
i
n
th
e
s
h
o
u
ld
er
j
o
in
t
th
a
t
s
lig
h
tl
y
co
m
p
licate
s
b
o
th
t
h
e
f
o
r
w
ar
d
an
d
in
v
er
s
e
k
in
e
m
at
ics.
5
.
1
.
D
-
H
Repre
s
ent
a
t
io
n
W
e
f
ir
s
t
estab
lis
h
th
e
j
o
in
t
co
o
r
d
in
ate
f
r
a
m
es
u
s
i
n
g
t
h
e
D
-
H
co
n
v
en
tio
n
s
as
s
h
o
w
n
i
n
Fi
g
u
r
e
3
.
T
h
e
1
i
i
A
m
atr
ix
f
o
r
ea
ch
j
o
in
ts
i
n
D
-
H
m
et
h
o
d
is
as f
o
llo
w
s
:
1
0
0
0
0
1
i
i
i
i
i
i
i
i
i
i
i
i
ii
i
i
i
i
i
C
C
C
S
S
a
C
S
C
C
S
C
a
S
A
S
C
d
(
1
1
)
Fig
u
r
e
3
.
Stan
f
o
r
d
R
o
b
o
t a
n
d
D
-
H
R
ep
r
esen
tatio
n
f
o
r
its
L
i
n
k
C
o
o
r
d
in
ate
S
y
s
te
m
D
-
H
p
ar
a
m
eter
s
f
o
r
Sta
n
f
o
r
d
lin
k
co
o
r
d
in
ate
s
y
s
te
m
s
h
o
w
n
i
n
T
ab
le1
.
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
A
N
ew A
p
p
r
o
a
ch
to
th
e
S
o
lu
ti
o
n
o
f R
o
b
o
t Ki
n
ema
tic
B
a
s
ed
o
n
R
ela
tive
.
.
.
(
Mo
h
a
mma
d
R
e
z
a
E
lh
a
mi
)
219
T
ab
le
1
.
E
s
tab
lis
h
in
g
li
n
k
co
o
r
d
in
ate
s
y
s
te
m
f
o
r
Sta
n
f
o
r
d
M
an
ip
u
lato
r
[
6
]
J
o
i
n
t
i
i
i
i
a
i
d
1
1
90
0
1
h
2
2
90
0
2
h
3
90
0
0
3
d
4
4
90
0
0
5
5
90
90
0
0
6
6
90
0
0
6
h
T
r
an
s
f
o
r
m
atio
n
m
atr
ice
s
f
r
o
m
th
e
b
ase
to
t
h
e
e
n
d
ef
f
ec
to
r
i.
e.
1
i
i
A
o
f
li
n
k
/j
o
in
t
i
r
elati
v
e
to
j
o
in
t
i
-
1
co
u
ld
b
e
o
b
tain
ed
as (
1
2
)
:
1
1
2
2
1
1
2
2
0
1
2
1
2
3
1
2
3
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
,,
0
1
0
0
1
0
0
0
1
0
0
0
1
0
0
0
1
0
0
0
1
C
S
C
S
S
C
S
C
A
A
A
h
h
d
(
1
2
)
4
4
5
5
6
6
4
4
5
5
6
6
3
4
5
4
5
6
6
0
0
0
0
0
0
0
0
0
0
0
0
,,
0
1
0
0
0
1
0
0
0
0
1
0
0
0
1
0
0
0
1
0
0
0
1
C
S
S
C
S
C
S
C
C
S
C
S
A
A
A
h
Hen
ce
,
th
e
HT
M
o
f
en
d
ef
f
ec
t
o
r
0
6
T
is
:
0
0
1
2
3
4
5
6
1
2
3
4
5
6
0
0
0
1
x
x
x
x
y
y
y
y
z
z
z
z
n
s
a
p
n
s
a
p
T
A
A
A
A
A
A
n
s
a
p
(
1
3
)
W
h
er
e
6
4
1
4
2
1
6
5
4
1
1
2
4
1
5
2
6
5
1
4
1
2
4
5
1
2
6
4
1
2
1
4
6
2
5
2
4
5
2
4
6
[
]
[
]
,
[
]
[
]
,
[]
x
y
z
n
C
S
S
C
C
C
S
S
C
S
C
C
S
C
C
S
n
S
S
C
C
S
C
S
C
S
S
C
S
C
C
S
C
n
S
C
C
S
S
S
S
C
C
(
1
4
)
6
5
4
1
4
1
2
1
5
2
6
4
1
1
2
4
6
5
4
1
4
2
1
2
5
1
6
4
1
4
2
1
6
2
5
2
5
4
2
4
6
[
(
)
]
[
]
,
[
(
)
]
[
]
,
[]
x
y
z
s
C
S
C
S
S
C
C
C
C
S
S
S
S
C
C
C
s
C
S
C
C
S
C
S
S
C
S
S
S
C
C
C
S
s
C
C
C
S
S
S
S
C
S
5
1
4
1
4
2
1
2
5
1
2
5
5
1
4
2
1
4
2
4
5
2
5
()
()
x
y
z
a
C
S
C
C
S
C
C
S
S
a
S
S
S
C
C
C
C
S
S
a
S
S
C
C
S
2
1
6
5
4
1
1
2
4
1
2
5
3
1
2
2
1
6
5
1
4
2
1
4
1
2
5
3
1
2
1
6
2
5
5
2
4
3
2
[
(
)
]
[
(
)
]
()
x
y
z
p
h
S
h
C
C
S
C
C
S
C
S
S
d
C
S
p
h
C
h
C
C
C
C
S
S
S
S
S
d
S
S
p
h
h
C
S
C
S
S
d
C
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
.
3
,
Sep
tem
b
er
2
0
1
6
:
2
1
3
–
2
2
2
220
5
.
2
.
T
he
Co
nv
enient
Appro
a
ch
T
h
e
co
o
r
d
in
ate
f
r
a
m
e
s
y
s
te
m
u
s
ed
f
o
r
P
o
s
t
-
c
u
r
r
en
t
r
eso
lu
tio
n
o
f
co
n
v
e
n
ie
n
t
m
et
h
o
d
s
h
o
wn
i
n
Fi
g
u
r
e
4
.
No
tice
th
at
w
e
h
a
v
e
a
d
if
f
e
r
en
t
f
r
a
m
e
at
ea
c
h
j
o
in
t,
as
zi
ax
is
m
u
s
t
l
ie
alo
n
g
th
e
j
o
in
t
i
o
r
d
i
f
o
r
r
ev
o
lu
te
an
d
p
r
is
m
atic
j
o
in
t,
r
esp
ec
tiv
e
l
y
.
Fig
u
r
e
4
.
Stan
f
o
r
d
L
in
k
C
o
o
r
d
in
ate
S
y
s
te
m
i
n
P
o
s
t
-
c
u
r
r
en
t
Ap
p
r
o
ac
h
T
r
an
s
f
o
r
m
atio
n
m
atr
ice
s
o
b
tain
ed
as f
o
llo
w
s
:
1
1
1
1
1
1
1
1
0
1
11
2
2
2
2
2
2
2
2
1
2
22
1
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
1
0
0
1
0
0
0
1
0
0
0
1
0
0
0
1
0
0
0
1
1
0
0
0
0
0
0
0
0
0
1
0
0
0
0
1
0
1
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
1
0
0
0
1
C
S
C
S
S
C
S
C
A
hh
C
S
C
S
h
S
C
h
A
SC
3
2
3
3
1
0
0
0
1
0
0
0
1
0
0
0
0
0
1
0
0
1
0
0
0
0
1
0
1
0
0
0
0
1
0
1
0
0
0
0
0
1
0
0
0
1
0
0
0
1
d
A
d
(
1
5
)
4
4
4
4
4
4
4
4
3
4
5
5
5
5
55
4
5
55
0
1
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
1
0
0
0
1
0
0
0
1
0
0
0
0
1
0
0
0
1
0
0
0
1
0
1
0
0
0
0
0
0
0
0
1
0
0
0
0
0
1
0
1
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
1
0
0
0
1
C
S
S
C
S
C
C
S
A
C
S
S
C
SC
A
CS
6
6
6
6
6
6
6
6
5
6
66
0
1
0
0
0
0
0
0
0
0
1
0
0
0
0
1
1
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
1
0
0
0
1
C
S
S
C
h
S
C
h
A
CS
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
A
N
ew A
p
p
r
o
a
ch
to
th
e
S
o
lu
ti
o
n
o
f R
o
b
o
t Ki
n
ema
tic
B
a
s
ed
o
n
R
ela
tive
.
.
.
(
Mo
h
a
mma
d
R
e
z
a
E
lh
a
mi
)
221
an
d
th
e
HT
M
o
f
en
d
ef
f
ec
to
r
0
6
T
is
ca
lcu
la
ted
as:
0
0
1
2
3
4
5
6
1
2
3
4
5
6
0
0
0
1
x
x
x
x
y
y
y
y
z
z
z
z
n
s
a
p
n
s
a
p
T
A
A
A
A
A
A
n
s
a
p
(
1
6
)
W
h
er
e
,
6
4
1
4
2
1
6
5
4
1
1
2
4
1
5
2
6
5
1
4
1
2
4
5
1
2
6
4
1
2
1
4
6
2
5
2
4
5
2
4
6
[
]
[
]
,
[
]
[
]
,
[]
x
y
z
n
C
S
S
C
C
C
S
S
C
S
C
C
S
C
C
S
n
S
S
C
C
S
C
S
C
S
S
C
S
C
C
S
C
n
S
C
C
S
S
S
S
C
C
(
1
7
)
6
5
4
1
4
1
2
1
5
2
6
4
1
1
2
4
6
5
4
1
4
2
1
2
5
1
6
4
1
4
2
1
6
2
5
2
5
4
2
4
6
[
(
)
]
[
]
,
[
(
)
]
[
]
,
[]
x
y
z
s
C
S
C
S
S
C
C
C
C
S
S
S
S
C
C
C
s
C
S
C
C
S
C
S
S
C
S
S
S
C
C
C
S
s
C
C
C
S
S
S
S
C
S
5
1
4
1
4
2
1
2
5
1
2
5
5
1
4
2
1
4
2
4
5
2
5
()
()
x
y
z
a
C
S
C
C
S
C
C
S
S
a
S
S
S
C
C
C
C
S
S
a
S
S
C
C
S
2
1
6
5
4
1
1
2
4
1
2
5
3
1
2
2
1
6
5
1
4
2
1
4
1
2
5
3
1
2
1
6
2
5
5
2
4
3
2
[
(
)
]
[
(
)
]
()
x
y
z
p
h
S
h
C
C
S
C
C
S
C
S
S
d
C
S
p
h
C
h
C
C
C
C
S
S
S
S
S
d
S
S
p
h
h
C
S
C
S
S
d
C
5
.
2
.
Co
m
pa
ri
ng
t
he
Resul
t
s
W
e
e
m
p
lo
y
ed
b
o
th
D
-
H
a
n
d
C
o
n
v
e
n
ien
t
ap
p
r
o
ac
h
to
co
m
p
ar
e
th
e
t
w
o
m
et
h
o
d
s
i
n
k
in
e
m
at
ics
an
al
y
s
is
o
f
Stan
f
o
r
d
m
a
n
ip
u
la
to
r
an
d
in
d
icate
th
e
ad
v
an
ta
g
e
s
o
f
o
u
r
p
r
o
p
o
s
ed
m
et
h
o
d
clea
r
l
y
.
C
o
m
p
ar
in
g
t
h
e
h
o
m
o
g
en
o
u
s
tr
an
s
f
o
r
m
a
tio
n
m
atr
i
x
o
f
th
e
en
d
ef
f
ec
to
r
,
0
6
T
i
n
eq
u
atio
n
s
(
1
4
)
,
(
1
7
)
an
d
,
w
e
ca
n
s
ee
th
at
all
ele
m
e
n
ts
f
o
r
m
i
n
g
th
e
m
atr
i
x
ar
e
ex
ac
tl
y
eq
u
a
l in
all
eq
u
a
tio
n
s
.
I
t
p
r
o
v
es
a
s
i
g
n
i
f
ica
n
t
m
ea
n
i
n
g
t
h
at
r
e
g
ar
d
less
o
f
w
h
at
co
o
r
d
in
ate
s
y
s
te
m
is
u
s
ed
,
th
e
e
n
d
-
ef
f
ec
to
r
p
o
s
itio
n
an
d
o
r
ien
tatio
n
w
o
u
ld
b
e
th
e
s
a
m
e
u
s
i
n
g
b
o
th
m
et
h
o
d
s
.
Usi
n
g
a
d
if
f
er
en
t
co
o
r
d
in
ate
s
y
s
te
m
i
n
co
n
v
en
ie
n
t
ap
p
r
o
ac
h
,
alth
o
u
g
h
th
e
i
n
d
iv
id
u
al
HT
Ms
f
o
r
j
o
in
ts
1
to
6
ar
e
d
if
f
er
en
t,
as
lo
n
g
as
th
e
f
ir
s
t
a
n
d
last
co
o
r
d
in
ate
s
y
s
te
m
s
ar
e
s
i
m
ilar
,
th
eir
m
u
ltip
licat
io
n
f
o
r
m
i
n
g
th
e
e
n
d
ef
f
ec
to
r
HT
M
is
t
h
e
s
a
m
e
as
p
r
ev
io
u
s
.
I
t
m
ea
n
s
t
h
at
i
n
co
n
v
en
ien
t
ap
p
r
o
ac
h
w
e
h
av
e
t
h
e
f
r
ee
d
o
m
to
ch
o
o
s
e
o
u
r
o
w
n
ar
b
itra
r
y
co
o
r
d
in
ate
s
y
s
te
m
w
it
h
o
n
l
y
o
n
e
s
i
m
p
le
r
u
le
to
b
e
i
m
p
le
m
e
n
ted
:
th
e
Z
i
ax
is
m
u
s
t
lie
a
lo
n
g
th
e
m
o
v
e
m
en
t
o
f
j
o
in
t
i
d
eg
r
ee
o
f
f
r
ee
d
o
m
.
T
h
er
ef
o
r
e,
th
e
d
e
v
astati
n
g
s
tep
s
o
f
co
o
r
d
in
ate
s
y
s
te
m
as
s
i
g
n
m
e
n
t
an
d
lin
k
/j
o
in
t
p
ar
am
e
ter
s
ex
tr
ac
tio
n
i
n
D
-
H
m
e
th
o
d
w
ill
n
o
lo
n
g
er
i
s
n
ee
d
ed
.
T
h
is
is
a
p
r
ef
ec
t r
ea
s
o
n
f
o
r
p
r
io
r
ity
o
f
“th
e
C
o
n
v
e
n
ie
n
t
A
p
p
r
o
ac
h
”
o
v
er
th
e
co
n
v
e
n
tio
n
al
D
-
H
r
ep
r
esen
tat
io
n
6.
C
O
NCLU
T
I
O
N
S
I
n
th
i
s
p
ap
er
,
R
elati
v
e
T
r
an
s
f
o
r
m
at
io
n
P
r
in
cip
le
(
R
T
P
)
h
as
b
ee
n
th
o
r
o
u
g
h
l
y
i
n
tr
o
d
u
ce
d
w
it
h
clea
r
co
n
ce
p
t
an
d
ex
a
m
p
le,
t
h
e
n
a
n
e
w
s
i
m
p
li
f
ied
an
d
ac
ce
s
s
ib
le
m
eth
o
d
ca
lled
“
T
h
e
C
o
n
v
en
i
en
t
A
p
p
r
o
ac
h
”
f
o
r
th
e
s
o
l
u
tio
n
o
f
r
o
b
o
t
k
i
n
e
m
ati
cs
h
a
s
b
ee
n
d
e
v
elo
p
ed
.
T
h
e
ad
v
an
ta
g
es
o
f
t
h
is
ap
p
r
o
ac
h
h
a
s
b
ee
n
d
is
c
u
s
s
ed
b
y
co
m
p
ar
i
n
g
w
it
h
t
h
e
w
el
l
-
k
n
o
w
n
D
-
H
r
ep
r
ese
n
tatio
n
t
h
r
o
u
g
h
p
er
f
o
r
m
i
n
g
t
h
e
f
o
r
w
ar
d
k
i
n
e
m
a
tic
o
n
Sta
n
f
o
r
d
in
d
u
s
tr
ial
r
o
b
o
t.
T
h
e
r
esu
l
ts
s
h
o
w
ed
th
e
co
n
s
is
te
n
c
y
o
f
t
h
e
n
e
w
m
et
h
o
d
w
it
h
m
u
ch
m
o
r
e
s
i
m
p
le
as
s
i
g
n
m
e
n
t
o
f
th
e
co
o
r
d
in
ate
f
r
a
m
ed
.
T
h
e
au
th
o
r
s
b
elie
v
e
t
h
at
b
y
s
tr
o
n
g
b
asi
s
o
f
p
r
i
n
cip
le
an
d
ea
s
y
ap
p
r
o
ac
h
o
f
ass
i
g
n
m
e
n
t,
th
is
m
et
h
o
d
w
o
u
l
d
b
ec
o
m
e
a
v
er
y
h
elp
f
u
l to
o
l i
n
r
o
b
o
tics
ed
u
ca
tio
n
an
d
en
g
i
n
ee
r
in
g
.
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
.
3
,
Sep
tem
b
er
2
0
1
6
:
2
1
3
–
2
2
2
222
RE
F
E
R
E
NC
E
S
[1
]
Re
u
lea
u
x
F
,
F
e
rg
u
so
n
ES
.
Kin
e
m
a
ti
c
s o
f
m
a
c
h
in
e
r
y
:
o
u
tl
in
e
s
o
f
a
th
e
o
ry
o
f
m
a
c
h
in
e
s: Co
u
r
ier Co
r
p
o
ra
ti
o
n
;
2
0
1
2
.
[2
]
De
n
a
v
it
J.
A
k
in
e
m
a
ti
c
n
o
tatio
n
f
o
r
lo
w
e
r
-
p
a
ir
m
e
c
h
a
n
ism
s
b
a
s
e
d
o
n
m
a
tri
c
e
s.
T
ra
n
s
o
f
t
h
e
AS
M
E
J
o
u
rn
a
l
o
f
Ap
p
li
e
d
M
e
c
h
a
n
ics
.
1
9
5
5
;2
2
:2
1
5
-
21.
[3
]
P
a
u
l
R
P
,
S
h
im
a
n
o
B,
M
a
y
e
r
GE.
Kin
e
m
a
ti
c
c
o
n
tro
l
e
q
u
a
ti
o
n
s
f
o
r
si
m
p
le
m
a
n
ip
u
lat
o
rs.
IEE
E
T
ra
n
sa
c
ti
o
n
s
o
n
S
y
ste
ms
,
M
a
n
,
a
n
d
Cy
b
e
rn
e
ti
c
s.
1
9
8
1
;1
1
:4
4
9
-
5.
[
4
]
P
a
u
l
R
P
,
S
h
im
a
n
o
B,
e
d
it
o
rs.
Ki
n
e
ma
ti
c
c
o
n
tr
o
l
e
q
u
a
t
io
n
s
fo
r
sim
p
le
ma
n
i
p
u
l
a
to
rs
.
De
c
isio
n
a
n
d
C
o
n
tro
l
in
c
l
u
d
i
n
g
th
e
1
7
th
S
y
m
p
o
siu
m
o
n
A
d
a
p
ti
v
e
P
ro
c
e
ss
e
s,
1
9
7
8
I
EE
E
C
o
n
f
e
re
n
c
e
o
n
;
1
9
7
9
:
IEE
E
.
[5
]
L
e
e
C
G
.
Ro
b
o
t
a
rm
k
in
e
m
a
ti
c
s,
d
y
n
a
m
i
c
s,
a
n
d
c
o
n
tr
o
l.
Co
m
p
u
ter.
1
9
8
2
;
1
5
(
1
2
):
6
2
-
8
0
.
[6
]
Cra
ig
JJ
.
In
tro
d
u
c
ti
o
n
t
o
r
o
b
o
ti
c
s:
m
e
c
h
a
n
ics
a
n
d
c
o
n
tro
l
:
P
e
a
rso
n
P
re
n
ti
c
e
Ha
ll
Up
p
e
r
S
a
d
d
le Ri
v
e
r;
2
0
0
5
.
[7
]
G
o
n
z
a
lez
RC,
F
u
K,
L
e
e
C.
Ro
b
o
ti
c
s: Co
n
tr
o
l,
S
e
n
si
n
g
,
Visio
n
,
a
n
d
In
t
e
ll
ig
e
n
c
e
.
Ed
it
o
ra
Ha
rd
c
o
v
e
r.
1
9
8
7
.
[8
]
P
a
u
l
R.
R
o
b
o
t
m
a
n
ip
u
lat
o
rs:
m
a
th
e
m
a
ti
c
s,
p
ro
g
ra
m
m
in
g
a
n
d
c
o
n
tr
o
l,
1
9
8
1
.
M
IT
P
re
ss
.
2
8
:3
0
7
-
1
6
.
[9
]
L
e
e
C
G
,
Zi
e
g
ler
M
.
G
e
o
m
e
tri
c
a
p
p
r
o
a
c
h
i
n
so
lv
i
n
g
in
v
e
rse
k
in
e
m
a
ti
c
s
o
f
P
UMA
ro
b
o
ts.
Aer
o
sp
a
c
e
a
n
d
El
e
c
tro
n
i
c
S
y
ste
ms
,
I
EE
E
T
ra
n
sa
c
ti
o
n
s
o
n
.
1
9
8
4
(6
):
6
9
5
-
7
0
6
.
[1
0
]
Ko
re
n
Y,
Ko
re
n
Y.
R
o
b
o
ti
c
s f
o
r
e
n
g
in
e
e
rs:
M
c
G
ra
w
-
Hill
Ne
w
Yo
r
k
e
t
a
l;
1
9
8
5
.
[1
1
]
S
icili
a
n
o
B,
S
c
iav
icc
o
L
,
V
il
lan
i
L
,
Orio
lo
G
.
Ro
b
o
ti
c
s:
m
o
d
e
ll
in
g
,
p
lan
n
i
n
g
a
n
d
c
o
n
tr
o
l:
S
p
ri
n
g
e
r
S
c
ien
c
e
&
Bu
sin
e
ss
M
e
d
ia;
2
0
1
0
.
[
1
2
]
T
sa
i
L
-
W
.
Ro
b
o
t
a
n
a
ly
sis: t
h
e
m
e
c
h
a
n
ics
o
f
se
rial
a
n
d
p
a
ra
ll
e
l
m
a
n
ip
u
lat
o
rs:
Jo
h
n
W
il
e
y
&
S
o
n
s; 1
9
9
9
.
[1
3
]
Cra
n
e
III
CD,
Du
f
fy
J.
Kin
e
m
a
ti
c
a
n
a
ly
sis o
f
ro
b
o
t
m
a
n
ip
u
lato
rs:
C
a
m
b
rid
g
e
Un
iv
e
rsit
y
P
re
ss
;
2
0
0
8
.
Evaluation Warning : The document was created with Spire.PDF for Python.