Contribution to the control of robot interacting with dynamic environment

Contribution to the control of robot interacting with dynamic environment

Preprints of the Fourth IFAC Symposium on Robot Control September 19-21, 1994, Capri, Italy CONTRIBUTION TO THE CONTROL OF ROBOT INTERACTING WITH DYN...

2MB Sizes 0 Downloads 25 Views

Preprints of the Fourth IFAC Symposium on Robot Control September 19-21, 1994, Capri, Italy

CONTRIBUTION TO THE CONTROL OF ROBOT INTERACTING WITH DYNAMIC ENVIRONMENT Miomir VukobratoviC* and Radoslav StojiC* and Yuri Ekalo·· • Robotics Department, Mihailo Pupin Institute, 11000 Belgrade, Yugoslavia •• Institute of Fine Mech. and Optics,197101, 51. Petersburg, Russia

Abstract - Based on a new approach to solving the control tasks for robots in contact with the environment, in this paper a generalization of the correct solution is given. The control task is set and solved in its general form, where the number of contact force components is smaller than the number of degrees of freedom of a robotic mechanism. The control methodology presented in the paper offers the possibility of synthesizing control laws, which ensure asymptotic stability of desired robot behaviour. Simultaneous stabilization of both desired robot motion and the desired interaction force with an in advance specified transient response is achieved for cases in which the environment dynamics is described by second order ilonlinear differential equations. Keywords: Robots, Nonlinear Control Systems, Force Control, Robot Dynamics

1

Introduction

(1)

The control of constrained motion of robots is a challenging research area, the successful solution of which will considerably affect further application of robots in industry and increase their efficiency and productivity. The increasing demand for advanced robot applications has brought about an enormous growth of interest in the development of different concepts and schemes for the control of compliant motion. Strong support for the study of this problem is derived from the new complex and somewhat unspecified robotic tasks such as assembly, machining or remote handling in an unstructured environment, where automation currently represents one of the most challenging topics in robotics. Based on the principle of closed-loop control systems stability, the control laws stabilizing simultaneously the robot motion and its interaction forces with the environment have been synthesized, (Ekalo and Vukobratovic 1993, 1994; Vukobratovic and Ekalo 1993). These control laws, unlike the control laws s}'nthesized by using the traditional approaches ( De Luca and Manes 1991; Hogan, 1985; Paul, 1987; Raibert and Craig, 1981; Yoshikawa et al., 1988) possess the exponential stability of closed-loop systems and ensure the required quality of transient responses. This paper can be considered as an extension and generalization of the results by Vukobratovic and Ekalo (1993). to cover the usual case when the number of contact force components is smaller than the number of manipulator degrees of freedom.

2

where q is an n-dimensional vector of generalized robot coordinates; H(q) is an n x n positive definite matrix of inertia of manipulator mechanism; h(q, q) is an n-dimensional nonlinear function of centrifugal, Coriolis' and gravitations terms; T is an n-dimensional vector of control inputs; F is an m-dimensional vector of the generalized forces acting on the end-effector from the environment; JT (q) is an n x m full rank matrix connecting vector F with the generalized forces associated to the generalized robot coordinates q. Let the dynamics model of the environment be described in the frame of robot coordinates as:

M(q)ii + L(q,q)

= -JT(q)F

(2)

where M(q) is a continuous n x n matrix representing environment inertia, L(q, q) is a continuous n-vector function. It is assumed that robot coordinates q and velocities q are sufficient to represent the state of the dynamic environment ( the case when the robot gripper tightly grasps an object representing the environment - see example at the end of the paper), and that the number of interacting forces is smaller than the number of robot degrees of freedom (DOF) ie. that m < n. Note that, by assumption, the interaction force F is uniquely determined by robot motion q(t). However, the converse is not true. Namely, M(q) is positive semidefinite and rankM(q) = m, so eq. (2) contains m independent differential equations only. Hence, for a given interaction force F(t), eq. (2) doesn't have a unique solution with respect to q and robot motion q(t) cannot be determined by integratingeq. (2). However, the interaction force F(t) fully determines the motion of the environment (see Remark 1. in Appendix).

Models of Robot and Environment Dynamics. Task Setting

Consider the model of a robot interacting with the environment, written in the form: 487

ij = P(7],~)

Therefore, it is more convenient to use an representation of environment dynamics in the form:

M(q)ij + L(q, q)

= -SI' (q)F

(3)

where M(q) is a continuous n x m full rank matrix, L( q, q) is a continuous m-vector function, and S(q) is a nonsingular continuous m x m matrix. The eq. (1) can be rewritten as:

T = U(q, q, ij, F)

= H(q)ij + h(q, q) -

JT(q)F (4)

or in the Cauchy form:

ij

= (q, q, T,

F)

= H- 1(q)[T+JT(q)F-h(q, q)](5)

If q1, q2 are subvectors of vector q, so that matrix M(q) can be partitioned intosubmatrices M 1, M2 such that:

M(q)ij

= Ml(q)ijl + M2(q)ij2

= 'li(q, q, ij1, F) (6) = -M2"Xq)[SI'(q)F + L(q,q) + Mt{qfjd It is assumed that the functions and 'li are

=

1t

Q(J.'(w))dw

(12)

jJ(t) = Q(J.'(t))

The goal of the control of a robot in contact with the environment is to realize the programmed motion qO(t) in the presence of the programmed interaction force FO(t), which satisfy the relation:

and specify the desired quality of the transient response by the equation:

(8)

(14)

It is possible to choose qO(t) and then from eq. (8) to determine FO(t), or alternatively, to spec-

where 7]1(t) = q1(t) - q?(t), P 1 is continuously differentiable (n - m)-dimensional function, such that eq. (14) has the solution 7]1 = O. The robot control should ensure that in the closed loop the perturbed robot motion and the perturbed robot interaction with the environment satisfyeqs. (13,14). The choice of function P 1 and Q ensures the asymptotic stability in the whole of the trivial solution of the systems (13,14).

ify FO(t) and programmed motion, q?(t), whereby subvector q~(t) can be obtained by integration of eq. (6) Tbe control task is: define control T which for the initial conditions q(to) f. qO(to), q(to) f. qO(to) , F(to) f. FO(to), satisfies the conditions: 00

(9)

00

(10)

(13)

However, because of eq. (6), achievement of PFI FO(t) stabilization can ensure the stabilization of m robot degrees of freedom only, represented by vector q2. So, for the force stabilization task to be well defined, it is also required to set the condition:

= f(q, q, ij) = -(SI' (q))-l(M(q)ij + L(q, q)) (7)

q(t) - qO(t), q(t) - qO(t), t F(t) - FO(t), t -

J.'(t)

where J.'(t) = F(t) - FO(t), Q is a continuously differentiable vector function characterizin~ the quality of transient responses. Equation (12) can be represented in the equivalent form:

continuously differentiable. The eq. (3) can be rewritten as:

F

where 7](t) = q(t) - qO(t), P is an n-dimensional vector function continuously differentiable with respect to all arguments, such that eq. (11) has the trivial solution 7] = O. The robot control should ensure that the equation of closed loop robot dynamics coincides with eq. (11). It is adopted a priori that the choice of function P ensures asymptotic stability in the whole of the trivial solution of the system (11). Analogously, the task of FO(t) stabilization can be posed to fulfil the condition (10) in such a way that the preset quality of transient response is defined by the integral equation (Vukobratovic and Ekalo 1993) :

to

with rankM2 (q) = m "Iq E nn, then eq. (3) can be written as:

q2

(11)

3

with the quality of transient response specified in advance. However, the transient response for both motion and force cannot be arbitrarily specified. Namely, due to eq. (7), the motion transient response fully determines the force transient process. On the other hand, the force transient process determines the motion transient process partially. For a given force transient response, the part of motion coordinates q1(t) has to be specified in order to determine the rest of motion coordinates from eq. (6). For these reasons, two subtasks can be identified. The first one is attaining the control goal (9,10) with a specified motion transient response, and the second one is attaining the control goal (9,10) with a specified transient response for force F(t) and motion coordinates q1(t). The task of stabilizing the qO(t) should be realized in such a way as to satisfy the desired quality of the transient response defined by the equation (Vukobratovic and Ekalo 1993):

Control Laws Stabilizing the Motion

Function P in (11), specifying the desired qual~ ity of transient response, can be adopted to be linear, so the system (11) obtains the form (Vukobratovic and Ekalo 1993): (15)

where r 1 , r 2 are constant n x n matrices. The system (15) can be rewritten in the form of a 2ndimensional system of differential equations:

:i: = rx where x

(16)

= (7]T, ~T)T,

r

= [~; f~]

.On,In

are the zero and unit n x n matrices respectively. Matrices r 1 , r2 can always be chosen to be such that the eigenvalues A1, ... , A2n of the matrix rare real, different, and negative. It is easy to check that one such choice is:

488

+ )..2,)..3 + )..4,"', )..2n-l + )..2n)

fl

= diag()..l

f2

= diag( -)..1)..2, -)..3)..4,"', -)..2n-l)..2n)

4

For simplicity, assume that function Q(J-l) is linear, so equation (13) takes the form:

Let us synthesize control law T(q, q, F, t) that ensure the desired quality of robot motion (11). From (11) it follows:

jJ = RJ-l

where R is m x m constant matrix. In (Vukobratovic and Ekalo 1993) the control laws have been proposed which ensure force stabilization for the case m = n, i.e. when the number of controlled interaction force components is equal to the robot DOF number. These control laws can easily be modified for the case considered here, i.e. when m < n. To define the control laws, introduce the following notation:

(17) Taking into account the robot dynamics model (1) the control law has to satisfy the equation:

T = H(q)(ijo

+ P(,." iJ)) + h(q, q) - JT (q)F

(18)

which can be rewritten in a more compact form:

T = U(q, q, ijo + P(,." iJ), F)

(19)

Control law (18) with feedback loops with respect to q, qand F solves the stated task of motion stabilization. This can be verified by substituting (18) into (1), and obtaining:

H(q)(ij - ijo - P(,." iJ)) = 0

IlFe =

->

0, i}(t)

->

0, t

ij

->

ijO(t), t

ijY + PI ("'1 , i}l) ] (24) 'P(q,q,ijY+Pl ("'l,iJd,Fo+IlFe) ij - [ ijY + PI ("'1, iJd ] (25) e2- 'P(q,q,ij~+Pl("'l,i}d,F)

The robot and environment dynamics equations (1,3) for each of the following control laws:

-> 00

TI = U(q, q, ijel, F) TIl = U(q, q, ije2, FO + IlFe) TIll = U(q, q, ijel, FO + IlFe)

-> 00

(26) (27) (28)

in a closed loop are equivalent to the reference equations (13,14).

Combining equations (3,6), (23-25), the implementable expressions for the control laws (26- 28) can be derived. The three control laws, defined by the statement, stabilize a desired interaction force with a specified quality of transient response. The proof of the Statement follows. The control laws (26, 27,28) can be obtained as a solution of the following algebraic equations:

T = U(q, q, ijo + P(,." i}), f(q, q, ijo + P(,." i}))) (21)

H(q)ijel + h(q,ij) M(q)ijel + L(q, q)

Substituting control law (21) into robot dynamics equation (1) the relation is obtained:

TI+JT(q)F

= -SI' (q)(FO + IlFe)

H(q)ije2 + h(q, ij) = TIl + JT (q)(FO M(q)ije2 + L(q,q) = -SI'(q)F

(H(q)+JT(q)(SI'(q)tlM(q))(ij-ijo - P(,." iJ)) = 0

+ JT(q)(SI'(q))-l M(q)):/; 0

Q(J-l(w))dw) (23)

and formulate the following Statement

Taking into account the continuity of the function f, it is finally obtained F(t) -> FO(t), t -> 00. In this way the control law (18) attains the control goal, stabilizing the motion with a prescribed quality of transient response, and also stabilizing the contact force FO(t), but the character of that transient response cannot be prescribed in advance in an arbitrary way. The next control law stabilizes the robot motion with no force feedback (Vukobratovic and Ekalo 1993):

If: det(H(q)

- [ el-

(20)

Due to the continuity of the function P, it follows:

ij(t)

it to

Since H(q) is a positive definite, and therefore nonsingular, consequently (20) is equivalent to (11). By assumption, function P(,." iJ) of ensure the asymptotic stability in the whole of the trivial solution ,., = 0 of the system (11), i.e.:

,.,(t)

Control Laws Stabilizing the Interaction Force

+ IlFe)

(29) (30) (31)

(32)

(22)

H(q)ijel + h(q, ij) = TIll + JT (q)(FO + IlFe) (33) M(q)ijel + L(q, q) = -SI' (q)(FO + IlFe) (34)

then equation of the closed loop system is equivalent to the reference equation (11). Condition (22) is always fulfilled for an arbitrary robot configuration ( see Remark 3 in the Appendix). Although the control law (21) does not use force feedback, it is explicitly dependent on the environment dynamics through the function f. Let us notice that the control law ( 19 ) takes into account the environment dynamics only implicitly, via the measured interaction force F and, hence, appears to be more attractive from this viewpoint. Moreover, when applying the control law (19) to solve the control task, the only supplementary information about the environment that is used is that the stability of motion implies the convergence of F(t) towards FO(t).

=

=

Substituting T Ti, (i I,Il,IlI) into (1,3) and subtracting (1,3) from (29) to (34), respectively, we obtain the relations:

H(q) [ M(q)

489

0 ] [ if - ifel ] - 0 SI'(q) J-l- IlFe -

(35)

H(q) [ M(q)

-JT(q)] [ if - ije2 ] 0 J-l- IlFe

=0

(36)

H(q) [ M(q)

-JT(q)] [ ij - ijel ] - 0 SI'(q) J-l- IlFe -

(37)

Then, system (43,45) can be rewritten as:

Since all system matrices in the above equations are nonsingular ( see remarks 4.,5,6. in Appendix), it follows that:

Xl X2

= =

P(xd K(X2,xl,t)+(3(X2,xl,t)1-'

ij - ijel = 0 ij - ijc2 = 0 I-' - 6.Fc = 0

Linearizing equation (48) in the vicinity of X = 0, it follows:

Using (23,24,25) it is finally obtained:

X2 = A(t)X2+B(t)Xl +0'(X2, Xl, t)+(3(X2, Xl, t)I-'(49)

ijl - ij? - Pl (171 , 1]1) = 0

( 41)

= it Q(I-'(w))dw)

(42)

I-'

where A(t) and B(t) are 2m x 2m and 2m x 2(nm) matrices, respectively, 0'(X2, Xl, t) = o(lIxll) when (x) --+ 0 and sup A(t) < 00 since qO(t), qO(t)

to

t

belong to bounded regions. The sufficient conditions for the asymptotic stability of the solution of the system of differential equations (48) are given by the following theorem. Theorem Let the dynamics of the environment satisfy the following conditions: 1. The linear system:

which finally proves that robot and environment with each of the control laws (26,27,28) behave according to the reference equations:

Consider now the question: is it sufficient to stabilize FO(t) in order to have the real motion approaching the programmed one i.e. q(t) --+ qO(t), as t --+ 00 ? Let any of the control laws (26) to (28) be applied to system (1, 3) . Using the environment dynamics model (3) and taking into account (8) we obtain :

(50)

is regular, i. e. the limit exists: 2n

lim -1- itTr A(w) dw = (10 t-+oo t - to to

M(q)ij - M(qO)ijo + L(q,q) - L(qO,qO) = -SI'(q)F + SI'(qO)FO

ih+K(17 , 1], t) = _M2"l(17+qO)SI'(17+qO)(F-FO)( 45)

(51)

where 172(t) = q2(t) - q~(t), and K(17, 1], t) = M2"l(17+qO){L(17+qO, 1]+qO)-L(qO, qO)+ + [M(17+qO)- M(qO)] ijo+[sT(17+qO)-sT(qO)] FO+ Ml (qO + 17)Pl (17l,1]l)} If F(t) = FO(t) this equation reduces to :

+ K(17, 1], t) =

and also equation (14) ensures that for its arbitrary solution 171 (t) the following estimate holds: (52)

with constants D 1, D2 > 0 and index .A satisfying inequality: -.A < min(O'.I;). If i satisfies relation :max.\: 0'.1; < - i < 0 then, for sufficiently small initial perturbations x(to) and I-'(to), the transient process of the system (48) satisfies the inequality:

(46)

0

Evidently, trivial solution 172(t) = 0 is stable only if the environment has such properties which ensure fulfilment of the condition q2(t) --+ q~(t), as t --+ 00 . Hence, if F(t) i= FO(t), 'Vt ~ to stabilization of FO(t) does not necessarily ensure stability of motion III contact with the environment. It is shown that the motion of robot and environment in a closed loop is described by the equations: ~1

IIx2(t)11 ~

5

P(Xl)

= [P(17~~1]d]

112]

172'

X = [ Xl ] X2'

, K(X2,Xl,t)

= [-K(~~iJ,t)]

(3(X2, Xl, t) = [ -Mi 1(17 + qO(t))sT(17 + qO)

Example

To illustrate the obtained theoretical results let us consider the following hypothetical contact task: the 2-DOF sliding joints manipulator has to move a tool over the support which behaves as a system with distributed parameters (Fig 1). The control goal consist of the realization of the nominal motion along x-axis xO(t) = vot, Vo const and nominal force along y-axis F~ = FO == const Component of contact force along x-axis, Fz; is the sum of friction terms:

Let us formulate sufficient conditions for the stabilization of the q~(t) . Introduce the notation :

X2 = [

'Vt ~ to

with some fositive constants a, b, c and consequently system (48) is exponentially stable. The proof of the Theorem can be obtained from the authors.

= P1(171 , 1]1) = -M2"l(17 + qO)SI'(17+qO)(F-FO)

111] 171'

(a ll x 2(t o)1I + bllxl(to~l~ ~1I1-'(t0)11) * *e--y(t-t o ),

~2+K(17, 1], t)

Xl = [

i=l

where 0'.1; (k = 1,2, ... , 2m) are characteristic indices of the solution of system (50), Tr A is trace of matrix A. 2. All characteristic indices 0'.1; (k = 1,2, ... , 2m) are negative. 3. Equation (13) ensures that for its arbitrary solution I-'(t) the following estimate holds:

Since, according to Statement, system (1,3) in a closed loop with control laws (26-28) is equivalent to the system in deviation form (13,14 ), the preceding relation reduces to the equation:

~2

(1o=LO'.I;

1 490

(61) has to be considered. It is easy to check that if conditions of Theorem are satisfied, i.e. eq (61) has characteristic values 0'1,0'2 < -, < 0, for some " and (xc , Wxc , Txc are chosen such that characteristic values of (57 ,58) are smaller than 0'1,0'2, then the closed loop system is stable. Note that example demonstrates that conditions of Theorem are not the necessary conditions for system stability.

6

Y

m

Fy

Figure 1: Robot in contact with environment (53) while contact force component Fy is sum of inertial , friction and elastic terms : (54)

It is adopted in (53,54) that my = me where me is the equivalent mass representing contribution of environment inertia. Further hy, hx, ky, 1/ denotes viscous friction, environment stiffness and static friction coefficients, respectively. Using the notation given in Fig 1 robot dynamic model is: (m1

+ m2)~ m2Y

=

Tx - Fx Ty - Fy

References De Luca, A. and C . Manes (1991). Hybrid force/position control for robots in contact with dynamic environments, in Proc. Robot Control, SYROCO '91, pp . 377- 382 .

(555 ) ( 6)

Ekalo, Y . and M. Vukobratovic (1993) . Robust and adaptive position/force stabilization conditions of robotic manipulators in contact tasks , Robotica, 11, 373-386 .

Introducing notation :

h(

.) - [ hxx q,q -

J(q)

=

+ l-'(hyY0+ kyy)sgnx

[-1 0] 0

-1

'

M ( q) =

]

Ekalo, Y . and M . Vukobratovic (to appear in 1994) . Adaptive stabilization of motion and forces in contact tasks for robotic manipulators with non-stationary dynamics, Int . J. of Rob. s and A utom . .

'

Ekalo, Y . and M. Vukobratovic (to appear in 1994) Quality of stabilization of robot interacting with dynamic environment, J. of Int. and Rob. Sys ..

[0 my),

Hogan, N. (1985). Impedance control : an approach to manipulation, part 1.- theory, part 2.implementation, part 3.- application , ASME 1. of Dyn . Syst., Meas . and Cont., 107, 1- 24.

sT(q) = [1] equations (53 - 56) reduce to form 0,3). Determine the remaining nominal motion component from (54) : yO(t) = t-Fo and choose equations specifying desired moti~n and force transient responses in the form :

Paul, R . (1987) . Problems and research issues associated with the hybrid control of force and displacements, in Proc . IEEE Int . Conf. on Rob. and Autom. 1966-1971, Raleigh, USA .

o o

Raibert, M. H. and J. J. Craig,(1981) Hybrid position/force control of manipulators, ASM E J. of Dyn. Sys., Meas. and Cont., 103, 126-133.

where T}x = x - xO, I-' = F - FO and (xc,w xc , Tyc are constants . Let us apply the control law (28) which in this case is:

Tx = (m1

Ty

+ m2)( -2(xcwxct}x - W;cT}x) + +hxx + 1/ (FO + J I-'dt) sgnx

= (:: +~ (FO+ JI-'dt) -

: : (hyY

+ kyy)

CONCLUSION

The control methodology presented in the paper exhibits the same features as in the basic one (Vukobratovic and Ekalo 1993), where the dimensions of control input vector and contact force vector are equal. Namely the synthesis of control laws is performed to ensure the asymptotic stability of the desired robot behaviour . Stabilizat ion is realized simultaneously of both desired robot motion and interaction force, for cases in which environment dynamics is described by second order nonlinear differential equations . All necessary modifications of control laws stabilizing the interaction force of the robot and environment have been performed and the corresponding stability theorem has been formulated .

Vukobratovic, M. and Y . Ekalo,(1993) Unified approach to control laws synthesis for robotic manipulators in contact with dynamic environment, in Tutorial S5, IEEE Int. Conf. on Rob. and Autom., pp . 213-229, Atlanta, USA.

(59)

Yoshikawa, T ., T . Sugie, and M. Tanaka,(1988) Dynamic hybrid position/force control of robot manipulators - control design and experiment, IEEE J. of Rob. and Autom., 4, 699-705 .

(60)

It is obvious that with this control law eqs . (57 ,58) represent the closed loop system behaviour . In order to examine nominal motion and force stability, th e en\·ironment equation (54) in deviation form:

APPENDIX:

Some Basic Relations for Robot in Contact with Dynamic Environment 491

Consider a robot with n degrees of freedom (DOF) whose configuration space is an n-dimensional differentiable manifold 'D'R. . Let the robot interact with a dynamic environment not possessing its own DOF . Then the environment configuration space is generally an m-dimensional submanifold 'De of manifold 'D'R. with m ~ n . In the text to follow all relations used are assumed to be valid everywhere on 'D'R. and 'De, respectively. Let q be the generalized robot coordinates ( local coordinates on 'D'R. ) and let s be the local coordinates on 'De . The robot dynamics can be described by the differential equation:

H(q)ij + h(q, ij)

= T +:F

(A.I)

+ £(S,5) + F = 0

= J(q)q. 0
(AA)

s = J(q)ij + j(q)q :F = JT(q)F

(A.6) (A.7)

+ M(
(A .14)

+ JT (q)(sT (q))-l M(q)

[!~!~

= T(A.16)

sT°(q)] is non-

The matrix: AIl(q) = [Z~~~ -J~(q)] is nonsingular. If ql, q2 are sub vectors of vector q such that: (A .17)

the matrix AIl(q) can be partitioned into submatrices in the following way:

(A. IS)

(A.S)

Left multiplying by JT(q), and taking into account (A.7) , the obtained equation reduces to:

M(q)ij + L(q , q) +:F = 0

(A .15)

singular. Since H(q) and sT(q) are nonsingular, from detAr(q) = det(H(q))det(S(q)) it follows that detAr(q) -# O. Remark 5.

(A.5)

0

H(q) = H(q)

The matrix: Ar(q) =

Substituting (A.3-A.6) into (A.2), (A.2) can be written in terms of generalized coordinates q as:

M (
M2(q) = sT(q)M(
The matrix of inertia H(q) of this system is positive definite by definition . Remark 4.

(A.3)

5 = ~
Remark 2. The matrix:

(H +JT(sT)-l M)ij+h+JT(sT)-l M L

(A.2)

=
(A.12) (A.13)

This statement can be verified if vector F is eliminated from eq. (A .12,A.13) , which results in:

where M(s) is a continuous mxm positive definite matrix, £(s, 5) is a continuous m-vector function, and F is a generalized interaction force vector in local coordinates on 'De . Since 'De C 'D'R. there exists a function

s

H(q)ij + h(q, ij) = T + JT (q)F M(q)ij+ L(q,q) = -sT(q)F

is positive definite.

where:F is an vector of the generalized interaction forces. The environment dynamics satisfies the equation:

M(s)s

Substituting (AA,A.6,A .7) into eq. (A .l,A.2) and left multiplying by sT (q) , the system equations reduce to:

Introduce: T(q) = [-S-l(q)S'(q)

o

~ ~ 1Then :

0 I rankAIl(q) = rank(rT(q)AIl(q)T(q)). Furtheron,

(A.9)

It is often useful to retain force F in external coordinates, and use instead of (A.I,A.9):

it is obtained that:

H(q)ij + h(q , ij) = T + JT(q)F M(q)ij + L(q, q) = _J T (q)F

0 hll(q) h12(q) rT AIlT = [ h21(q) h22(q) -sTO(q) o M2(q)

(A.IO)

1

(A.19)

h 12 (q)] is positive h22(q)

Remark 1. The matrix M(q) representing the

where the matrix: [hll(q)

environment inertia in robot generalized coordinates is positive-semidefinite. Since rankJ(q) = m, there exists a nonzero vector q such that J(q)q = 0, so M(q) is not pos-

definite having all principal minors positive definite . From (A.19) it follows :

h21(q)

det( rT AIlT) = -det(h ll )det(M2)det(S) (A.20) Since matrices M2(q), hll(q) are nonsingular:

itive definite . Due to the positive definiteness of m x m matrix M (
rankM(q) = m

detAIl = det(rT AIlT)

-# 0

Remark 6.

(A.ll)

The matrix: ArIl(q) = [ nonsingular.

The environment dynamics equation can be rearranged in the following way. Due to eq. (A.5), a submatrix S(q) of J(q) exists such that rankS(q) = m so that matrix J(q) can be partitioned into submatrices: J(q) = [S'(q) I S(q)]"

(A.2I)

!~!~

-JT(q)] . IS sT(q)

Since:

+ JT(sT)-l M) implies detArIl(q) -# 0

detArIl) = det( H remark 3. 492

det( sT )