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
.
5
4
~
6
0
I
SS
N:
2089
-
4856
54
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
Va
lida
tion o
f
End
Eff
ec
tor Ma
trix
f
o
r Robo
tic
K
it
O
WI
-
535
using
MAT
LAB
a
nd Robo
Analy
z
e
r
B
hiv
ra
j
Su
t
ha
r
*
,
T
a
n
m
a
y
S
hriv
a
s
t
a
v
a
**
,
L
a
m
y
a
nb
a
H
ei
s
na
m
***
*
D
e
p
a
rt
m
e
n
t
o
f
M
e
c
h
a
n
ica
l
En
g
in
e
e
rin
g
,
In
d
ian
I
n
stit
u
te o
f
T
e
c
h
n
o
lo
g
y
De
lh
i
,
In
d
ia
**
D
e
p
a
rt
m
e
n
t
o
f
M
e
c
h
a
n
ica
l
En
g
in
e
e
rin
g
,
S
h
ri
G
o
v
in
d
ra
m
S
e
k
sa
ri
y
a
In
stit
u
te o
f
S
c
ien
c
e
a
n
d
T
e
c
h
n
o
lo
g
y
,
In
d
o
re
*
*
*
D
e
p
a
rt
m
e
n
t
o
f
M
e
c
h
a
n
ica
l
En
g
in
e
e
rin
g
,
Na
ti
o
n
a
l
I
n
stit
u
te o
f
T
e
c
h
n
o
l
o
g
y
Ha
m
irp
u
r
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
T
h
is
p
a
p
e
r
p
re
se
n
ts
th
e
v
a
li
d
a
ti
o
n
o
f
th
e
e
n
d
e
ff
e
c
to
r
m
a
tri
x
h
a
v
in
g
D
-
H
p
a
ra
m
e
ter o
f
th
e
4
DO
F
e
d
u
c
a
ti
o
n
a
l
m
a
n
ip
u
lato
r
OW
I
-
5
3
5
b
y
M
ATLA
B
a
n
d
Ro
b
o
A
n
a
ly
z
e
r.
T
ra
n
s
f
o
rm
a
ti
o
n
m
a
tri
x
o
f
o
rd
e
r
4
x
4
w
h
ich
d
e
s
c
rib
e
s
e
n
d
e
ffe
c
ter‟s
p
o
siti
o
n
a
n
d
o
rien
tati
o
n
w
it
h
re
sp
e
c
t
to
th
e
b
a
se
re
f
e
r
e
n
c
e
f
r
a
m
e
.
M
AT
LAB
p
ro
g
ra
m
m
in
g
w
h
ich
g
iv
e
s d
e
tails ab
o
u
t
th
e
tran
sla
ti
o
n
s
tep
s o
f
th
e
m
a
n
ip
u
lat
o
r
sim
u
lt
a
n
e
o
u
sly
.
F
o
rw
a
rd
k
i
n
e
m
a
ti
c
s
o
f
O
W
I
5
3
5
ro
b
o
ti
c
k
it
h
a
s
b
e
e
n
c
a
lcu
late
d
b
y
M
A
TL
A
B
a
s
w
e
ll
a
s
Ro
b
o
A
n
a
l
y
z
e
r.
W
e
h
a
d
c
a
lcu
late
d
th
e
En
d
e
ff
e
c
to
r
m
a
tri
x
in
b
o
th
so
f
twa
re
a
n
d
c
o
m
p
a
re
it
.
We
f
o
u
n
d
th
a
t
th
e
re
su
lt
s
a
re
si
m
il
a
r
u
p
to
th
re
e
d
ig
it
in
so
m
e
e
le
m
e
n
ts
a
n
d
u
p
to
tw
o
d
ig
it
s
f
ro
m
d
e
c
i
m
a
l
in
fe
w
e
le
m
e
n
ts
a
n
d
it
w
a
s
d
iff
e
r
e
n
t
a
f
ter
th
re
e
d
ig
i
t
f
ro
m
th
e
d
e
c
ima
l
in
e
n
d
e
ff
e
c
to
r
m
a
tri
x
.
K
ey
w
o
r
d
:
E
d
u
ca
tio
n
al
R
o
b
o
tic
Kit
OW
I
535
MA
T
L
A
B
R
o
b
o
A
n
al
y
ze
r
So
f
t
w
ar
e
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
:
B
h
iv
r
aj
Su
t
h
ar
,
Dep
ar
te
m
en
t o
f
Me
ch
a
n
ical
E
n
g
i
n
ee
r
i
n
g
,
I
n
d
ian
I
n
s
tit
u
te
o
f
T
ec
h
n
o
lo
g
y
Delh
i,
I
n
d
ia
E
m
ail
:
b
h
iv
r
aj
.
iitd
@
g
m
ai
l.c
o
m
1.
I
NT
RO
D
UCT
I
O
N
T
h
e
k
in
e
m
atic
o
f
t
h
e
m
a
n
ip
u
lato
r
r
ef
er
s
to
h
o
w
to
ca
lcu
la
te
a
p
o
s
itio
n
an
d
o
r
ien
tatio
n
o
f
th
e
en
d
ef
f
ec
to
r
an
d
to
g
et
th
e
d
esire
d
co
n
f
i
g
u
r
atio
n
o
f
t
h
e
m
a
n
ip
u
l
ato
r
.
Dy
n
a
m
ic
p
r
o
p
er
ties
,
s
u
c
h
as
w
ei
g
h
t,
in
er
tia
etc.
,
ar
e
n
o
t
co
n
s
id
er
ed
as
p
ar
t
o
f
t
h
e
k
i
n
e
m
atics
[
1
]
.
Kin
e
m
atic
m
o
d
el
d
escr
ib
es
th
e
s
p
ati
al
p
o
s
itio
n
o
f
j
o
in
t
s
an
d
lin
k
s
,
a
n
d
p
o
s
itio
n
an
d
o
r
ien
tatio
n
o
f
th
e
e
n
d
-
e
f
f
ec
to
r
[
2
,
3
,
4
]
.
T
h
e
r
ep
r
esen
tatio
n
o
f
th
e
r
o
b
o
t‟
s
e
n
d
-
ef
f
ec
to
r
p
o
s
itio
n
a
n
d
o
r
ien
tat
io
n
t
h
r
o
u
g
h
th
e
g
eo
m
etr
ies
o
f
r
o
b
o
ts
(
j
o
in
t
an
d
lin
k
p
ar
a
m
eter
s
)
ar
e
ca
lled
Fo
r
w
ar
d
Kin
e
m
atics.
Us
in
g
Fo
r
w
ar
d
Kin
e
m
atics,
t
h
e
m
a
th
e
m
atica
l
m
o
d
el
is
d
ev
elo
p
ed
to
c
o
m
p
u
te
th
e
p
o
s
itio
n
a
n
d
o
r
ien
tatio
n
o
f
e
n
d
-
e
f
f
ec
to
r
‟
s
b
ased
o
n
th
e
g
iv
en
R
o
b
o
t
j
o
in
t
p
o
s
itio
n
.
E
ac
h
R
o
b
o
t
j
o
in
t
is
co
n
s
id
er
ed
as
r
ev
o
lu
te
j
o
in
t.
T
h
e
h
o
m
o
g
en
o
u
s
tr
an
s
f
o
r
m
at
io
n
o
f
en
d
-
e
f
f
ec
to
r
r
elate
d
to
th
e
b
ase
f
r
a
m
e
is
f
o
r
m
u
lated
u
s
i
n
g
De
n
a
v
it
-
Har
ten
b
er
g
(
D
-
H)
m
e
th
o
d
[
5
]
.
A
r
o
b
o
tic
m
a
n
ip
u
lato
r
is
d
e
s
ig
n
e
d
to
p
er
f
o
r
m
a
ta
s
k
in
th
e
3
-
D
s
p
ac
e.
T
h
e
to
o
l
o
r
en
d
-
e
f
f
ec
to
r
is
to
f
o
llo
w
a
p
lan
n
ed
tr
aj
ec
to
r
y
to
m
an
ip
u
late
o
b
j
ec
ts
o
r
ca
r
r
y
o
u
t
th
e
tas
k
i
n
th
e
w
o
r
k
s
p
ac
e.
T
h
is
r
eq
u
ir
e
s
co
n
tr
o
l
o
f
p
o
s
iti
o
n
o
f
ea
ch
li
n
k
an
d
j
o
in
t
o
f
th
e
m
an
ip
u
lato
r
to
co
n
tr
o
l
b
o
th
th
e
p
o
s
itio
n
an
d
o
r
ien
tatio
n
o
f
th
e
to
o
l.
T
o
p
r
o
g
r
a
m
th
e
to
o
l
m
o
tio
n
a
n
d
j
o
in
t
li
n
k
m
o
t
io
n
s
,
a
m
at
h
e
m
a
tic
al
m
o
d
el
o
f
th
e
m
a
n
ip
u
lato
r
i
s
r
eq
u
ir
ed
to
r
ef
er
t
o
all
g
eo
m
etr
ical
a
n
d
/o
r
ti
m
e
-
b
ased
p
r
o
p
er
ties
o
f
th
e
m
o
tio
n
[
6
]
.
T
h
is
p
ap
er
ad
o
p
ted
a
f
o
r
w
ar
d
k
in
e
m
atic
s
m
o
d
el
p
r
ed
icate
d
(
DH)
an
al
y
tic
al
s
ch
e
m
e
f
o
r
r
o
b
o
t
ar
m
p
o
s
itio
n
p
lace
m
e
n
t.
T
h
e
d
ev
elo
p
ed
m
o
d
el
ai
m
s
a
t
p
r
ed
ictin
g
an
d
r
ec
o
v
er
in
g
th
e
e
n
d
-
ef
f
ec
ter
p
o
s
itio
n
o
f
OW
I
5
3
5
R
o
b
o
t
m
an
ip
u
lato
r
f
o
r
d
if
f
er
en
t
j
o
in
t
v
ar
iab
les;
f
in
all
y
in
t
h
e
en
v
ir
o
n
m
en
t
o
f
MA
T
L
A
B
,
th
e
f
o
r
w
ar
d
k
i
n
e
m
atic
s
m
o
d
el
is
b
u
ilt to
tak
e
k
i
n
e
m
atics iter
atio
n
b
y
u
s
i
n
g
M
A
T
L
A
B
.
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
V
a
lid
a
tio
n
o
f E
n
d
E
ffecto
r
Ma
tr
ix
fo
r
R
o
b
o
tic
K
it O
W
I
-
5
3
5
u
s
in
g
MATLAB
a
n
d
.
.
.
(
B
h
ivra
j S
u
th
a
r
)
55
2.
RO
B
O
T
DE
SCR
I
P
T
I
O
N
OW
I
-
5
3
5
R
o
b
o
t
m
a
n
ip
u
lato
r
u
s
ed
i
n
th
e
w
o
r
k
is
a
4
D
OF
r
o
b
o
t
ar
m
m
a
n
ip
u
lato
r
s
h
o
w
n
i
n
f
i
g
.
(
1
)
.
I
t is a
f
o
u
r
ar
tic
u
lated
co
o
r
d
in
ate
r
o
b
o
tic
m
a
n
ip
u
lato
r
t
h
at
u
s
es D
C
m
o
to
r
s
w
it
h
w
o
r
m
g
ea
r
f
o
r
j
o
in
t a
ct
u
ato
r
s
,
an
d
its
m
o
tio
n
ar
e
co
n
tr
o
lled
b
y
w
ir
ed
r
e
m
o
te.
OW
I
-
5
3
5
R
o
b
o
t
m
a
n
ip
u
lato
r
h
a
s
a
s
tatio
n
ar
y
b
ase,
s
h
o
u
ld
er
,
elb
o
w
an
d
w
r
is
t
i
n
co
r
r
esp
o
n
d
in
g
w
i
th
h
u
m
a
n
ar
m
j
o
in
ts
,
ea
ch
o
f
t
h
ese
j
o
in
t
h
a
s
a
s
i
n
g
le
DOF.
W
r
is
t
ca
n
m
o
v
e
i
n
to
p
lan
es,
t
h
is
m
a
k
i
n
g
th
e
en
d
-
ef
f
ec
to
r
m
o
v
e
s
m
o
o
th
l
y
in
ter
m
s
o
f
o
b
j
ec
t m
a
n
ip
u
la
tio
n
.
Fig
u
r
e
1
.
C
A
D
Mo
d
el
o
f
Fo
u
r
ax
is
r
o
b
o
tic
Kit O
W
I
-
535
OW
I
-
5
3
5
is
th
e
4
DOF
ed
u
ca
tio
n
al
s
er
ia
l
m
an
ip
u
lato
r
s
w
h
ic
h
ca
n
b
e
u
tili
ze
to
p
ic
k
-
p
lace
&
h
o
ld
th
e
co
m
m
o
d
itie
s
w
ith
i
n
th
e
w
o
r
k
in
g
v
o
lu
m
e.
A
ll
t
h
e
f
o
u
r
j
o
in
t
m
o
v
e
m
en
t
s
ar
e
r
o
tar
y
w
h
ich
m
ak
e
t
h
e
m
o
d
el
o
f
co
n
f
i
g
u
r
atio
n
4
R
o
r
R
-
R
-
R
-
R
t
y
p
e.
2
.
1
.
F
o
rwa
rd
k
ine
m
a
t
ics o
f
ro
bo
t
ic
k
it
O
WI
-
535
I
n
th
i
s
p
ap
er
w
e
ar
e
u
s
i
n
g
f
o
u
r
lin
k
r
o
b
o
ts
s
h
o
w
n
in
F
ig
u
r
e
3
.
I
n
o
r
d
er
to
f
in
d
r
elatio
n
b
et
w
ee
n
f
ir
s
t
lin
k
a
n
d
las
t
li
n
k
w
e
f
ix
ed
th
e
b
ase.
T
h
is
ca
n
b
e
o
b
t
ain
ed
f
r
o
m
th
e
d
escr
ip
tio
n
o
f
t
h
e
co
o
r
d
in
ate
tr
an
s
f
o
r
m
atio
n
s
b
et
w
ee
n
t
h
e
c
o
o
r
d
in
ate
f
r
a
m
e
s
attac
h
ed
to
a
ll
th
e
li
n
k
s
a
n
d
f
o
r
m
in
g
th
e
o
v
er
all
d
escr
ip
tio
n
i
n
a
r
ec
u
r
s
iv
e
m
an
n
er
.
Fo
r
th
is
p
u
r
p
o
s
e,
th
e
p
o
s
itio
n
an
d
o
r
ien
t
atio
n
o
f
th
e
r
ig
id
b
o
d
y
is
u
s
ef
u
l
f
o
r
o
b
tain
in
g
t
h
e
co
m
p
o
s
i
tio
n
o
f
co
o
r
d
in
ate
tr
an
s
f
o
r
m
atio
n
s
b
et
w
ee
n
t
h
e
co
n
s
ec
u
ti
v
e
f
r
a
m
e
s
.
A
s
a
f
ir
s
t
s
te
p
,
th
is
m
et
h
o
d
is
to
b
e
d
er
iv
ed
to
d
ef
in
e
t
h
e
r
elati
v
e
p
o
s
itio
n
a
n
d
o
r
ien
tatio
n
o
f
t
w
o
s
u
cc
e
s
s
i
v
e
li
n
k
s
.
T
h
e
p
r
o
b
lem
is
to
d
ef
i
n
e
t
w
o
f
r
a
m
e
s
attac
h
ed
to
t
w
o
s
u
cc
es
s
iv
e
lin
k
s
an
d
m
a
k
e
t
h
e
co
o
r
d
in
ate
tr
an
s
f
o
r
m
atio
n
b
et
w
ee
n
t
h
e
m
.
I
t
i
s
co
n
v
e
n
ien
t to
s
et
s
o
m
e
r
u
les
f
o
r
th
e
d
ef
in
it
io
n
o
f
t
h
e
li
n
k
f
r
a
m
es.
T
h
e
DH
p
ar
a
m
e
ter
s
co
r
r
esp
o
n
d
in
g
to
t
h
is
4
DOF
OW
I
-
5
3
5
r
o
b
o
t m
a
n
ip
u
la
to
r
ar
e
s
h
o
w
n
i
n
T
ab
le
1
.
Her
e
θi
is
th
e
j
o
in
t
an
g
le,
b
i
is
j
o
in
t
o
f
f
s
et,
ai
i
s
lin
k
le
n
g
th
,
an
d
α
i
is
t
h
e
t
w
i
s
t
a
n
g
le.
T
h
e
l
i
m
it
s
o
f
ea
ch
o
f
th
e
j
o
in
t
an
g
le
s
h
av
e
a
ls
o
b
ee
n
g
i
v
en
in
th
e
tab
le
an
d
t
h
e
s
e
l
i
m
its
ar
e
also
u
s
ed
i
n
t
h
e
M
A
T
L
A
B
p
r
o
g
r
a
m
m
i
n
g
.
T
h
e
Den
av
it
-
Har
te
n
b
er
g
(
DH)
co
n
v
en
tio
n
a
n
d
m
et
h
o
d
o
lo
g
y
is
u
s
ed
to
d
er
iv
e
its
f
o
r
w
ar
d
k
i
n
e
m
a
tics
.
W
e
u
s
ed
t
h
e
ab
o
v
e
m
en
t
io
n
s
et
o
f
j
o
in
t
o
f
f
-
s
et
„
b
‟
,
j
o
in
t
an
g
le
„
‟
,
li
n
k
len
g
t
h
„
a‟
,
t
wis
t
-
an
g
le
„
α
‟
o
n
l
y
v
ar
y
i
n
g
th
e
j
o
in
t a
n
g
le
f
r
o
m
m
in
i
m
u
m
v
al
u
e
to
m
a
x
i
m
u
m
v
al
u
e
w
it
h
th
e
t
w
o
i
n
ter
m
ed
iate
v
al
u
e
s
.
T
ab
le
1
.
D
-
H
P
ar
am
eter
s
o
f
O
W
I
-
5
3
5
r
o
b
o
tic
k
it
As
p
er
as
th
e
s
k
e
leto
n
k
in
e
m
atic
f
i
g
u
r
e
3
w
e
ca
n
s
ee
t
h
at
t
h
e
t
w
is
t
an
g
le
“
α
”
i.e
o
r
ien
t
atio
n
o
f
z
-
ax
is
o
f
t
w
o
j
o
in
ts
alo
n
g
x
d
ir
ec
tio
n
(
w
e
ca
n
s
a
y
th
at
a
n
g
le
b
et
w
ee
n
z
-
ax
i
s
)
o
f
o
w
i
-
5
3
5
m
o
d
el
is
9
0
o
,
0
o
,
0
o
,
0
o
r
esp
ec
tiv
el
y
f
o
r
th
e
j
o
in
t 1
,
j
o
in
t 2
,
j
o
in
t3
,
j
o
in
t 4
.
T
h
e
w
o
r
k
in
g
li
n
k
s
m
a
y
o
n
l
y
a
lter
th
e
j
o
in
t
an
g
le
s
o
w
e
s
e
lec
t
j
o
in
t
an
g
les
f
o
r
th
e
d
if
f
er
en
t
j
o
in
ts
as
a
v
ar
iab
le
p
ar
am
e
ter
to
p
er
f
o
r
m
th
e
an
al
y
s
is
to
o
b
tain
t
h
e
tr
an
s
f
o
r
m
atio
n
m
atr
i
x
.
R
e
v
o
l
u
t
e
J
o
i
n
t
b
(
m
m
)
m
i
n
(
d
e
g
.
)
m
ax
(
d
e
g
.
)
a
(
m
m
)
α
(
d
e
g
.
)
Jo
i
n
t
1
0
0
o
2
7
0
o
44
90
o
Jo
i
n
t
2
0
0
o
3
0
0
o
91
0
o
Jo
i
n
t
3
0
0
o
1
8
0
o
1
2
0
0
o
Jo
i
n
t
4
0
0
o
1
2
0
o
94
0
0
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
:
5
4
–
6
0
56
Fig
u
r
e
3
.
Kin
e
m
a
tic
s
c
h
e
m
a
ti
c
d
iag
r
a
m
o
f
t
h
e
r
o
b
o
tic
k
it O
W
I
-
535
2
.
2
.
T
ra
ns
f
o
r
m
a
t
io
n
m
a
t
ri
x
Af
ter
es
tab
lis
h
in
g
(
D
-
H)
co
o
r
d
in
ate
s
y
s
te
m
f
o
r
ea
ch
l
in
k
,
a
h
o
m
o
g
en
eo
u
s
tr
a
n
s
f
o
r
m
atio
n
m
atr
i
x
ca
n
ea
s
il
y
b
e
d
ev
elo
p
ed
co
n
s
id
er
in
g
f
r
a
m
e
{i
-
1
}
an
d
f
r
a
m
e
{i}
tr
an
s
f
o
r
m
a
tio
n
co
n
s
i
s
t
in
g
o
f
f
o
u
r
b
asic
tr
an
s
f
o
r
m
atio
n
s
.
T
h
er
e
f
o
u
r
p
ar
a
m
eter
s
ar
e
r
esp
ec
tiv
el
y
j
o
in
t
an
g
le
θi,
lin
k
o
f
f
s
et
b
i
,
lin
k
l
en
g
t
h
ai
an
d
th
e
t
w
i
s
t
a
n
g
le
α
i
,
So
,
t
h
e
lin
k
tr
an
s
f
o
r
m
atio
n
m
atr
i
x
b
et
w
ee
n
co
o
r
d
in
ate
f
r
a
m
e
i
-
1
a
n
d
co
o
r
d
in
ate
f
r
a
m
e
i.
MA
T
L
A
B
co
d
e
f
o
r
tr
an
s
f
o
r
m
atio
n
m
atr
i
x
h
a
s
b
ee
n
g
iv
e
n
in
ap
p
en
d
ix
.
3.
VE
RIF
I
CA
T
I
O
N
O
F
M
AT
H
E
M
AT
I
CAL
M
O
DE
L
US
I
NG
M
AT
L
AB
1
0
.
0
P
RO
G
RAM
WI
T
H
RO
B
O
ANALY
Z
E
R
SO
F
T
WARE
T
h
e
s
i
m
u
lat
io
n
r
esu
lts
as
p
r
esen
ted
ar
e
f
o
r
th
e
f
o
r
w
ar
d
k
in
e
m
atic
an
al
y
s
is
o
f
th
e
OW
I
5
3
5
R
o
b
o
t
as
m
o
d
elled
u
s
in
g
th
e
(
D
-
H)
co
n
ce
p
t.
Si
m
u
latio
n
s
w
er
e
co
n
d
u
cted
u
s
i
n
g
M
A
T
L
A
B
co
d
i
n
g
o
n
an
I
n
te
l
(
R
)
C
P
U
T
2
0
8
0
@
0
.
9
9
GHz
,
1
.
0
0
GB
Me
m
o
r
y
(
R
A
M)
,
3
2
b
it Op
er
atin
g
S
y
s
te
m
.
T
h
e
M
A
T
L
A
B
co
d
in
g
w
as
u
s
ed
to
d
o
m
at
h
e
m
a
tica
l
i
ter
atio
n
s
o
f
th
e
s
er
ial
l
in
k
m
a
n
ip
u
lato
r
.
T
h
e
v
ar
iab
les
θ1
,
θ2
,
θ3
,
an
d
θ4
r
es
p
ec
tiv
el
y
r
ep
r
esen
t
th
e
j
o
in
t
ax
e
s
1
th
r
o
u
g
h
4
.
Kin
e
m
atic
s
eq
u
atio
n
s
f
r
o
m
t
h
e
o
v
er
all
tr
an
s
f
o
r
m
atio
n
m
a
tr
ix
w
er
e
d
ev
elo
p
ed
u
s
i
n
g
t
h
e
M
A
T
L
A
B
R
1
0
.
0
.
An
co
d
e
h
as
b
ee
n
d
ev
e
lo
p
ed
to
g
e
n
er
ate
t
h
e
f
o
r
w
ar
d
k
i
n
e
m
atic
s
eq
u
at
io
n
s
an
d
ca
lcu
late
t
h
e
r
o
b
o
t
Ma
n
ip
u
lat
o
r
p
o
s
itio
n
an
d
o
r
ien
tatio
n
in
ter
m
s
o
f
j
o
in
t
an
g
le
s
an
d
its
o
u
tp
u
t
is
co
m
p
ar
ed
w
it
h
R
o
b
o
t
s
o
f
t
w
ar
e
(
w
h
ic
h
is
th
e
s
i
m
u
late
p
r
o
g
r
am
s
u
p
p
l
ied
w
ith
t
h
e
r
o
b
o
t
s
y
s
te
m
)
f
o
r
f
o
u
r
s
ets
o
f
j
o
in
t
p
ar
am
eter
s
.
T
h
e
r
esu
lt
o
f
en
d
-
e
f
f
ec
ter
‟
s
p
o
s
itio
n
f
r
o
m
MA
T
L
A
B
iter
atio
n
w
as
t
h
e
n
co
m
p
ar
ed
w
it
h
ex
p
er
i
m
e
n
tal
r
e
s
u
lt
g
en
er
ated
f
r
o
m
i
n
b
u
ilt
R
o
b
o
An
al
y
ze
r
s
o
f
t
w
ar
e.
Fo
r
d
if
f
er
en
t
k
e
y
b
o
ar
d
v
alu
e
s
e
n
ter
ed
o
n
th
e
R
o
b
o
An
al
y
ze
r
s
o
f
t
w
ar
e,
th
e
co
r
r
esp
o
n
d
in
g
j
o
in
t
a
n
g
le
s
,
s
i
m
u
latio
n
an
d
e
x
p
er
i
m
e
n
t
al
p
o
s
itio
n
s
f
o
r
th
e
en
d
-
e
f
f
ec
ter
ar
e
p
r
esen
ted
.
3
.
1
.
I
np
ut
s
et
s
o
f
D
-
H
v
a
lues
:
W
e
h
av
e
li
n
k
le
n
g
th
,
o
f
f
s
ett
an
d
t
w
is
t
an
g
les
f
o
r
t
h
e
m
an
ip
u
lato
r
ar
e
f
i
x
ed
s
o
w
e
ca
n
v
ar
y
t
h
e
j
o
in
t
an
g
le
h
e
n
ce
t
h
ese
ar
e
t
h
e
d
i
f
f
er
en
t
i
n
p
u
t
s
et
s
o
f
D
-
H
p
ar
a
m
eter
s
to
M
A
T
L
A
B
a
n
d
R
o
b
o
An
al
y
ze
r
f
o
r
d
if
f
er
e
n
t v
a
lu
e
s
o
f
j
o
in
t a
n
g
le
ar
e
as f
o
llo
w
s
T
ab
le
3
: D
-
H
s
et1
P
ar
a
m
eter
s
o
f
d
i
f
f
er
en
t j
o
in
ts
f
o
r
m
in
T
ab
le
4
: D
-
H
s
et2
P
ar
am
eter
s
o
f
d
if
f
er
en
t j
o
in
ts
f
o
r
interm
edia
te1
R
e
v
o
l
u
t
e
Jo
i
n
t
b
(
mm
)
m
i
n
(
d
e
g
.
)
a
(
mm
)
α (d
e
g
.
)
Jo
i
n
t
1
0
60
o
44
90
o
Jo
i
n
t
2
0
60
o
91
0
o
Jo
i
n
t
3
0
60
o
1
2
0
0
o
Jo
i
n
t
4
0
60
o
94
00
R
e
v
o
l
u
t
e
Jo
i
n
t
b
(
mm
)
m
i
n
(
d
e
g
.
)
a
(
mm
)
α (d
e
g
.
)
Jo
i
n
t
1
0
0
o
44
90
o
Jo
i
n
t
2
0
0
o
91
0
o
Jo
i
n
t
3
0
0
o
1
2
0
0
o
Jo
i
n
t
4
0
0
o
94
0
0
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
V
a
lid
a
tio
n
o
f E
n
d
E
ffecto
r
Ma
tr
ix
fo
r
R
o
b
o
tic
K
it O
W
I
-
5
3
5
u
s
in
g
MATLAB
a
n
d
.
.
.
(
B
h
ivra
j S
u
th
a
r
)
57
T
ab
le
5
: D
-
H
s
et3
P
ar
am
eter
s
o
f
d
if
f
er
en
t j
o
in
ts
f
o
r
interm
edia
te2
R
e
v
o
l
u
t
e
Jo
i
n
t
b
(
mm
)
m
i
n
(
d
e
g
.
)
a
(
mm
)
α (d
e
g
.
)
Jo
i
n
t
1
0
1
0
0
o
44
90
o
Jo
i
n
t
2
0
1
0
0
o
91
0
o
Jo
i
n
t
3
0
80
o
1
2
0
0
o
Jo
i
n
t
4
0
80
o
94
0
0
T
ab
le
6
: D
-
H
s
et4
P
ar
am
eter
s
o
f
d
if
f
er
en
t j
o
in
ts
f
o
r
m
ax
R
e
v
o
l
u
t
e
Jo
i
n
t
b
(
mm
)
m
i
n
(
d
e
g
.
)
a
(
mm
)
α
(
d
e
g
.
)
Jo
i
n
t
1
0
2
7
0
o
44
90
o
Jo
i
n
t
2
0
3
0
0
o
91
0
o
Jo
i
n
t
3
0
1
8
0
o
1
2
0
0
o
Jo
i
n
t
4
0
1
2
0
o
94
0
0
P
u
ttin
g
th
e
s
e
ab
o
v
e
m
e
n
tio
n
s
ets o
f
v
al
u
e
s
an
d
o
b
tain
i
n
g
r
es
u
lts
f
o
r
tr
an
s
f
o
r
m
at
io
n
m
atr
ix
in
M
A
T
L
A
B
.
4.
O
UT
P
UT
F
RO
M
T
H
E
SE
T
S O
F
D
-
H
VALUE
S
4
.
1
M
AT
L
AB
R
esu
lt
s
Fo
r
th
e
s
et
1
f
o
r
m
in
Fo
r
th
e
s
et
2
interm
ediate1
Fo
r
th
e
s
et
3
interm
ediate2
Fo
r
th
e
s
et
4
m
ax
4
.
2
.
R
o
bo
Ana
ly
ze
r
Co
nfig
ura
t
io
n Set
s
Fo
r
th
e
s
et
1
m
in
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
:
5
4
–
6
0
58
Fo
r
th
e
s
et
2
interm
ediate1
Fo
r
th
e
s
et
3
interm
ediate2
Fo
r
th
e
s
et
4
m
ax
4
.
2
.
Ro
bo
Ana
ly
ze
r
R
esu
lt
s
Fo
r
th
e
s
et
1
f
o
r
m
in
Fo
r
th
e
s
et
2
interm
ediate1
Fo
r
th
e
s
et
3
interm
ediate2
Evaluation Warning : The document was created with Spire.PDF for Python.
IJ
RA
I
SS
N:
2089
-
4856
V
a
lid
a
tio
n
o
f E
n
d
E
ffecto
r
Ma
tr
ix
fo
r
R
o
b
o
tic
K
it O
W
I
-
5
3
5
u
s
in
g
MATLAB
a
n
d
.
.
.
(
B
h
ivra
j S
u
th
a
r
)
59
Fo
r
th
e
s
et
4
m
ax
5.
CO
NCLU
SI
O
N
& D
I
SCU
S
SI
O
N
T
h
e
o
u
tp
u
t
o
f
th
e
tr
a
n
s
f
o
r
m
atio
n
m
atr
ice
s
f
r
o
m
t
h
e
v
ar
i
ed
D
-
H
p
ar
a
m
eter
j
o
in
t
an
g
l
es
f
o
r
th
e
ed
u
ca
tio
n
al
OW
I
-
2
3
5
m
an
i
p
u
lato
r
f
o
r
th
e
r
esu
l
t
o
f
M
AT
L
A
B
h
ad
co
m
e
o
u
t
to
b
e
s
a
m
e
as
f
o
r
r
esu
lt
tr
an
s
f
o
r
m
atio
n
m
atr
ices
f
r
o
m
th
e
R
o
b
o
-
an
al
y
ze
r
.
B
u
t
p
o
i
n
t
to
b
e
n
o
t
ed
th
at
t
h
e
M
A
T
L
AB
r
esu
lts
ar
e
s
a
m
e
w
it
h
R
o
b
o
An
al
y
ze
r
r
esu
lt
s
u
p
to
o
n
l
y
t
w
o
p
lace
s
a
f
ter
d
ec
i
m
al
t
h
e
t
h
ir
d
d
ig
it
a
f
ter
d
ec
i
m
al
v
ar
ied
in
s
o
m
e
ele
m
e
n
ts
o
f
t
h
e
M
A
T
L
A
B
m
atr
ice
s
o
b
tain
ed
&
t
h
e
MA
T
L
A
B
g
i
v
es
th
e
r
e
s
u
l
t
co
m
p
o
n
e
n
t
s
o
f
t
h
e
tr
an
s
f
o
r
m
atio
n
m
a
tr
ices
u
p
to
f
o
u
r
p
lace
s
a
f
ter
d
ec
i
m
al
b
u
t
as
f
o
r
s
a
m
e
co
n
f
ig
u
r
atio
n
o
f
m
an
ip
u
lato
r
R
o
b
o
-
an
al
y
ze
r
g
iv
e
s
r
esu
lt
an
d
m
atr
ix
ele
m
e
n
t
s
h
as
d
i
g
ital
u
p
to
s
ix
p
lace
s
f
r
o
m
d
ec
i
m
a
l
in
tr
an
s
f
o
r
m
at
io
n
m
atr
ices.
T
h
at
m
ea
n
s
if
r
esear
ch
er
h
ad
D
-
H
p
ar
a
m
eter
s
v
al
u
es
an
d
n
ee
d
t
o
ev
alu
ate
th
e
tr
an
s
f
o
r
m
atio
n
m
atr
i
x
th
e
y
s
h
o
u
ld
b
e
p
r
ef
er
R
o
b
o
-
an
al
y
ze
r
r
ath
er
th
a
n
t
h
e
M
A
T
L
AB
f
o
r
ac
cu
r
ac
y
p
o
in
t o
f
v
ie
w
.
As
th
e
ab
o
v
e
co
n
c
lu
s
io
n
s
tate
s
th
at
t
h
er
e
is
v
ar
iatio
n
s
tar
ts
f
r
o
m
t
h
e
th
ir
d
p
lace
af
ter
th
e
d
ec
i
m
al
in
th
e
ab
o
v
e
u
s
ed
s
o
f
t
w
ar
e
‟
s
r
e
s
u
lt
v
alu
e
s
,
s
o
f
o
r
p
r
ec
is
io
n
wo
r
k
th
er
e
is
n
ee
d
to
d
ef
in
e
t
h
e
co
r
r
ec
t
v
alu
e
f
o
r
m
th
e
t
h
ir
d
p
lace
o
f
d
ec
i
m
al
o
b
tain
ed
f
r
o
m
d
i
f
f
er
en
t
s
o
f
t
w
ar
e
‟
s
.
W
e
n
ee
d
an
o
t
h
er
s
o
f
t
w
ar
e
o
r
an
o
th
er
co
d
in
g
p
latf
o
r
m
to
f
i
n
d
m
o
r
e
ac
cu
r
ate
r
esu
lts
.
A
PP
END
I
X
MA
T
L
A
B
P
R
O
G
R
A
M
F
o
r
e
st
i
mat
i
n
g
t
h
e
t
r
a
n
sf
o
r
mat
i
o
n
m
a
t
r
i
x
t
h
e
g
e
n
e
r
a
l
c
o
d
e
f
o
r
a
4
R
c
o
n
f
i
g
u
r
a
t
i
o
n
m
a
n
i
p
u
l
a
t
o
r
i
s
syms b1 theta1 a1 alpha1;
Qb1= [1 0 0 0;0 1 0 0;0 0 1 b1;0 0 0 1];
Qtheta1= [cos(theta1)
-
sin(theta1) 0 0;sin(theta1) cos(theta1) 0 0;0 0 1 0;0 0 0 1];
Qa1= [1
0 0 a1;0 1 0 0;0 0 1 0;0 0 0 1];
Qalpha1= [1 0 0 0;0 cos(alpha1)
-
sin(alpha1) 0 ;0 sin(alpha1) cos(alpha1) 0;0 0 0 1];
Qdh1= Qb1*Qtheta1*Qa1*Qalpha1
syms b2 theta2 a2 alpha2;
Qb2= [1 0 0 0;0 1 0 0;0 0 1 b2;0 0 0 1];
Qtheta2= [cos(theta2)
-
sin(theta2) 0 0
;sin(theta2) cos(theta2) 0 0;0 0 1 0;0 0 0 1];
Qa2= [1 0 0 a2;0 1 0 0;0 0 1 0;0 0 0 1];
Qalpha2= [1 0 0 0;0 cos(alpha2)
-
sin(alpha2) 0 ;0 sin(alpha2) cos(alpha2) 0;0 0 0 1];
Qdh2= Qb2*Qtheta2*Qa2*Qalpha2
syms b3 theta3 a3 alpha3;
Qb3= [1 0 0 0;0 1 0 0;0
0 1 b3;0 0 0 1];
Qtheta3= [cos(theta3)
-
sin(theta3) 0 0;sin(theta3) cos(theta3) 0 0;0 0 1 0;0 0 0 1];
Qa3= [1 0 0 a3;0 1 0 0;0 0 1 0;0 0 0 1];
Qalpha3= [1 0 0 0;0 cos(alpha3)
-
sin(alpha3) 0 ;0 sin(alpha3) cos(alpha3) 0;0 0 0 1];
Qdh3= Qb3*Qtheta3*Qa3*Qalph
a3
syms b4 theta4 a4 alpha4;
Qb4= [1 0 0 0;0 1 0 0;0 0 1 b4;0 0 0 1];
Qtheta4= [cos(theta4)
-
sin(theta4) 0 0;sin(theta4) cos(theta4) 0 0;0 0 1 0;0 0 0 1];
Qa4= [1 0 0 a4;0 1 0 0;0 0 1 0;0 0 0 1];
Qalpha4= [1 0 0 0;0 cos(alpha4)
-
sin(alpha4) 0 ;0 sin(alph
a4) cos(alpha4) 0;0 0 0 1];
Qdh4= Qb4*Qtheta4*Qa4*Qalpha4
Qdhdof4= Qdh1*Qdh2*Qdh3*Qdh4
RE
F
E
R
E
NC
E
S
[1
]
M
a
rk
W
.
S
p
o
n
g
,
"
Ro
b
o
t
M
o
d
e
li
n
g
a
n
d
Co
n
tro
l"
,
1
st E
d
it
i
o
n
,
Jo
h
n
W
il
e
y
&
S
o
n
s,
2
0
0
5
.
[2
]
M
it
tal
R.
K.
&
Na
g
ra
th
I.
J,
"
Ro
b
o
ti
c
s
&
Co
n
tr
o
l"
,
F
irst
Ed
it
io
n
,
T
a
ta
M
c
G
ra
w
-
Hill
P
u
b
l
ish
i
n
g
Co
.
L
td
.
,
p
p
7
0
-
1
0
7
,
2
0
0
6
.
[3
]
Nik
u
S
a
e
e
d
B,
"
In
tro
d
u
c
ti
o
n
to
R
o
b
o
ti
c
s
–
A
n
a
l
y
sis,
S
y
ste
m
s &
Ap
p
li
c
a
ti
o
n
s"
,
F
irst
Ed
it
i
o
n
,
P
e
a
rso
n
Ed
u
c
a
ti
o
n
P
v
t.
L
td
,
2
0
0
3
.
[4
]
S
c
h
il
in
g
R
o
b
e
rt
J,
“
F
u
n
d
a
m
e
n
tals o
f
Ro
b
o
ti
c
s”
,
P
re
n
ti
c
e
-
Ha
ll
o
f
In
d
ia
P
v
t.
L
id
,
p
p
2
5
-
7
6
,
2
0
0
9
.
[5
]
Jo
h
n
J.Cra
ig
,
”
In
tro
d
u
c
ti
o
n
to
Ro
b
o
ti
c
s”
.
2
n
d
e
d
.
P
e
a
rs
o
n
E
d
u
c
a
ti
o
n
In
ter
n
a
ti
o
n
a
l,
2
0
0
5
.
[6
]
Co
rk
e
,
P
.
I.
,
"
A
Ro
b
o
ti
c
s T
o
o
lb
o
x
f
o
r
M
ATLA
B
"
,
Nin
th
re
lea
se
,
S
p
rin
g
e
r
P
u
b
li
s
h
in
g
Co
,
S
e
p
tem
b
e
r
2
0
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
:
5
4
–
6
0
60
B
I
O
G
RAP
H
I
ES
O
F
AUTH
O
R
B
h
iv
r
a
j
S
u
t
h
a
r
,
I
h
a
d
c
o
m
p
lete
d
B.
E.
in
E
lec
tri
c
a
l
En
g
in
e
e
rin
g
a
n
d
p
o
st
g
ra
d
u
a
ted
in
E
n
e
rg
y
a
n
d
En
v
ir
o
n
m
e
n
t
M
a
n
a
g
e
m
e
n
t
fro
m
In
d
ian
In
sti
tu
te
o
f
T
e
c
h
n
o
lo
g
y
De
lh
i,
In
d
ia.
I
h
a
v
e
m
o
re
th
a
n
4
y
e
a
rs‟
e
x
p
e
rien
c
e
in
ro
b
o
ti
c
s,
M
e
c
h
a
tro
n
ics
,
A
u
to
m
a
ti
o
n
,
Co
n
tr
o
l
sy
ste
m
d
e
si
g
n
o
f
m
a
n
ip
u
lat
o
r,
W
e
a
ra
b
le
ro
b
o
t,
Re
h
a
b
il
it
a
ti
o
n
,
Hu
m
a
n
m
a
c
h
in
e
in
terf
a
c
e
,
T
e
le
-
o
p
e
ra
ti
o
n
o
f
In
d
u
strial
ro
b
o
t
.
I
w
o
rk
e
d
a
s
a
p
ro
jec
t
a
ss
o
c
iate
a
t
IIT
D
e
lh
i
in
t
h
e
a
re
a
o
f
re
n
e
w
a
b
le
e
n
e
r
g
y
,
M
icro
h
y
d
ro
s
y
ste
m
d
e
sig
n
f
o
r
o
n
e
y
e
a
r.
I
a
lso
w
o
rk
e
d
a
s
a
Ju
n
io
r
Re
se
a
rc
h
F
e
ll
o
w
p
ro
jec
t
ti
tl
e
d
”
T
e
le
o
p
e
ra
ti
o
n
o
f
In
d
u
stri
a
l
ro
b
o
t”
t
h
a
t
w
a
s
jo
in
t
p
r
o
jec
t
o
f
B
A
RC
a
n
d
IIT
De
lh
i.
I
h
a
v
e
o
n
e
j
o
u
r
n
a
l
p
a
p
e
r,
n
in
e
c
o
n
f
e
re
n
c
e
p
a
p
e
rs
in
Na
ti
o
n
a
l
a
n
d
In
tern
a
ti
o
n
a
l
c
o
n
f
e
re
n
c
e
s,
a
n
d
f
o
u
r
p
a
ten
ts.
I
a
lso
g
o
t
G
YT
I
Aw
a
rd
-
2
0
1
5
a
t
Ra
striap
a
ti
Bh
a
w
a
n
Ne
w
De
lh
i,
In
d
ia an
d
sh
o
rt
li
ste
d
in
to
p
1
0
g
o
o
d
p
r
o
jec
ts
f
o
r
“
IIT
De
l
h
i
8
9
Clas
s o
f
In
n
o
v
a
ti
o
n
Aw
a
rd
-
2
0
1
5
a
n
d
Be
st Res
e
a
r
c
h
p
a
p
e
r
a
wa
rd
in
A
n
In
tern
a
ti
o
n
a
l
Co
n
f
e
re
n
c
e
&
Ex
h
ib
it
io
n
o
n
C
u
tt
i
n
g
Ed
g
e
T
e
c
h
n
o
lo
g
i
cal
Ch
a
ll
e
n
g
e
s
in
M
e
c
h
a
n
ica
l
E
n
g
in
e
e
rin
g
a
t
No
id
a
In
sti
tu
te
o
f
En
g
in
e
e
rin
g
&
T
e
c
h
n
o
l
o
g
y
,
G
r
e
a
ter
No
id
a
,
IND
IA
.
Evaluation Warning : The document was created with Spire.PDF for Python.