![](bg3.png)
null space (Oh et al., 1998; Park et al., 2002; Ne-
mec and Zlajpah, 2000; Nemec and Zlajpah, 2001),
it enables good performance in real implementation
(Nemec et al., 2007). The joint space control law is
τ
c
= HJ
T
(
¨
x
x
x
d
+ K
v
˙
e
e
e
x
+ K
p
e
e
e
x
+ K
f
e
e
e
f
−
˙
J
˙
q
q
q) +
HN(
¨
q
q
q
nd
+ K
n
˙
e
e
e
n
−
˙
N
˙
q
q
q) + h
h
h+ J
T
f
f
f (9)
where J is the Jacobian matrix, H is n× n inertia ma-
trix, h
h
h is n-dimensional vector of the centrifugal, cori-
olis and gravity forces, f
f
f is n-dimensional vector of
the external forces acting on the manipulator’s end ef-
fector and K
p
, K
v
, K
f
and K
n
are diagonal matrices
representing positional, velocity, force and null-space
feedback gains. The first term of the control law cor-
responds to the task-space control torque τ
x
, the sec-
ond to the null-space control torque τ
n
and the third
and the fourth is used to compensate the non-linear
system dynamics and the external force, respectively.
Here, e
e
e
x
= x
x
x
d
− x
x
x are the task-space tracking error,
e
e
e
f
= f
f
f
d
− f
f
f and
˙
e
e
e
n
=
˙
q
q
q
nd
−
˙
q
q
q
n
is the null-space track-
ing error. x
x
x
d
, f
f
f
d
and
˙
q
q
q
nd
are the desired task coordi-
nates, desired force and the desired null space veloc-
ity, respectively. The details of the control law deriva-
tion can be found in (Nemec and Zlajpah, 2000; Ne-
mec et al., 2007).
An attention should be paid on the selection of the
inertia of the virtual link. The inertia matrix H has the
form
H =
H
r
0
0 H
v
(10)
where H
r
is the robot inertia matrix and H
v
is the di-
agonal matrix describing the virtual mechanism iner-
tia. Clearly, H
v
can not be zero, but arbitrary small
values can be chosen describing the lightweight vir-
tual mechanism. Selection of the inertia matrix of the
virtual mechanism affects only the null space behav-
ior of the whole system. Heavy virtual links with high
inertia would slow down the movements of the virtual
links. Therefore, low inertia of virtual links is an ap-
propriate choice. On the contrary, we can assume that
no gravity, coriolis and centrifugal forces act on the
virtual links and the corresponding terms in the vector
h
h
h can be set to zero. Control law 9 assumes feedback
from all joints, including non-existing virtual joints.
There are multiple choices how to provide the joint
coordinates and the joint velocities of the virtual link.
A suitable method is to build a simple model com-
posed of a double integrator
˙
q
q
q
v
=
Z
H
−1
v
τ
cv
(11)
q
q
q
v
=
Z
˙
q
q
q
v
where τ
cv
is the part of the control signals correspond-
ing to the virtual link.
4 MOTION OPTIMIZATION
As we mentioned previously, one of the main prob-
lems in automatic trajectory generation is the inabil-
ity to assure that the generated trajectory is feasible
using a particular robot, either because of possible
collisions with the environment or because of the lim-
ited workspace of the particular robot. Limitations in
the workspace are usually not subjected to the tool
position, but rather to the tool orientation. Another
sever problem are wrist singularities, which can not
be predicted in the trajectory design phase on a CAD
system. A widely used solution in such cases is off-
line programming with graphical simulation, where
such situation can be detected in the design phase
of the trajectory. Unfortunately this is a tedious and
time consuming process and therefore not applicable
in customized production, where almost each work
piece can vary from the previous one (Dulio and Boer,
2004; Nemec and Zlajpah, 2008). The problem can be
efficiently solved using the null space motion, which
changes the robot configuration, but does not affect
the task space motion. The force and the position
tracking are of the highest priority for a force con-
trolled robot and are therefore considered as the pri-
mary task. The secondary task is defined by the opti-
mization of a given cost function.
Let p be the desired cost function, which has to be
maximized or minimized. Then the velocities
˙
q
q
q
n
= kNH
−1
(
δp
δq
1
,
δp
δq
2
, .....
δp
δq
n
, ) (12)
in Eq 12 maximize cost function for any k > 0 and
minimize cost function for any k < 0 (Asada and Slo-
tine, 1986), where k is an arbitrary scalar which de-
fines the optimization step. In our case we have se-
lected a compound p which maximizes the distances
between obstacle and the robot links or robot work
object, maximizes the distance to the singular con-
figuration of the robot and maximizes the distance in
joint coordinates between current joint angle and joint
angle limit. We define the cost function as a sum
of three cost functions p = p
a
+ p
l
+ p
s
, where p
a
denotes cost function for obstacle avoidance, p
l
cost
function for avoiding joint limits and p
s
cost function
for singularity avoidance. We select the cost func-
tion for obstacle avoidance as (Khatib, 1986; Khatib,
1987) p
a
=
1
2
Ed
2
0
, where E is an l × l rotation matrix
describing the direction of an artificial potential field
pointing from the obstacle, l is the dimension of the
position sub-space and d
0
is the shortest distance be-
tween the obstacle and the robot body. In our case the
desired objective is fulfilled if the imaginary force is
applied only on the robot joints. The cost function for
the joint limits avoidance is defined as (Nemec and
SHOE GRINDING CELL USING VIRTUAL MECHANISM APPROACH
161