9/16/2015
13
Nguyen Q. Hoang, Nguyen V. Khang
49
Application of pseudo-inverse: Inverse kinematics
solving for
o
q
Determ. q
q
q
q
(0)
o
x
x
( )
t
x
( )
( )
t
t
x
x
1
s
Adjusting
(
)
(
)
o
q
J x
Ke
E
J J z
*
q
x
Method of adjustment in error of joint variables
Main idea of this method:
find q closed to q*, such that q satisfy constraint equation f(x,q)=0
Nguyen Q. Hoang, Nguyen V. Khang
50
Application of pseudo-inverse: Numerical experiments
q
1
O
g
E
x
y
A
B
C
q
2
q
3
q
4
C
1
C
2
C
3
C
4
q
5
D
Link
1
2
3
4
5
m [kg]
10
7.5
5
4
1
I
z
[kgm
2
]
0.11
0.10
0.03
0.02 0.02
l [m]
0.55
0.50
0.45
0.40 0.20
a [m]
0.15
0.16
0.19
0.20 0.10
A 5-DOF planar manipulator
Manipulator parameter
In this simulation, the end-effector (link 5)
will be forced to move at the velocity of 2
m/s along a cirlular trajectory, while its
orientation is constant, 1.0 rad. The
trajectory has a center at point (0.8, 0.5) m
and radius of R = 0.5 m.
0
2
4
6
8
10
0
0.5
1
1.5
2
t [s]
x
1
[m]
x
2
[m]
x
3
[rad]
Nguyen Q. Hoang, Nguyen V. Khang
51
Application of pseudo-inverse: Numerical experiments
A. Without consideration of joint limitations (planar manipulator)
0
2
4
6
8
10
-2
-1
0
1
2
3
t [s]
q
[
ra
d
]
q
1
q
2
q
3
q
4
q
5
0
2
4
6
8
10
-1
-0.5
0
0.5
1
1.5
x 10
-15
t [s]
e
1
[m]
e
2
[m]
e
3
[rad]
0
2
4
6
8
10
-30
-20
-10
0
10
20
30
t [s]
u
[
N
m
]
u
1
u
2
u
3
u
4
u
5
0
2
4
6
8
10
-2
-1
0
1
2
t [s]
q
[
ra
d
]
q
1
q
2
q
3
q
4
q
5
0
2
4
6
8
10
-30
-20
-10
0
10
20
30
t [s]
u
[
N
m
]
u
1
u
2
u
3
u
4
u
5
0
2
4
6
8
10
-1
-0.5
0
0.5
1
1.5
x 10
-15
t [s]
e
1
[m]
e
2
[m]
e
3
[rad]
B. Consideration of joint limitations (planar manipulator)
Nguyen Q. Hoang, Nguyen V. Khang
52
Application of pseudo-inverse:
(
)
q
J x
Ke
(
) [
( ) ( )]
0
q
J x
Ke
E
J q J q z
q
J x
[
( ) ( )]
0
q
J x
E
J q J q z
Numerical experiments / without post-adjusting
