An Explicit Formulation of Singularity-Free Dynamic Equations ... - Core

Modeling, Identication and Control, Vol. 33, No. 2, 2012, pp. 4560, ISSN 18901328

An Explicit Formulation of Singularity-Free Dynamic Equations of Mechanical Systems in Lagrangian FormPart one: Single Rigid Bodies Pål Johan From

Department of Mathematical Sciences and Technology, Norwegian University of Life Sciences, 1432 Ås, Norway. E-mail: [email protected]

Abstract This paper presents the explicit dynamic equations of a mechanical system. The equations are presented so that they can easily be implemented in a simulation software or controller environment and are also well suited for system and controller analysis. The dynamics of a general mechanical system consisting of one or more rigid bodies can be derived from the Lagrangian.

We can then use several well known

properties of Lie groups to guarantee that these equations are well dened.

This will, however, often

lead to rather abstract formulation of the dynamic equations that cannot be implemented in a simulation software directly. In this paper we close this gap and show what the explicit dynamic equations look like. These equations can then be implemented directly in a simulation software and no background knowledge on Lie theory and dierential geometry on the practitioner's side is required. This is the rst of two papers on this topic. In this paper we derive the dynamics for single rigid bodies, while in the second part we study multibody systems. In addition to making the equations more accessible to practitioners, a motivation behind the papers is to correct a few errors commonly found in literature. For the rst time, we show the detailed derivations and how to arrive at the correct set of equations. We also show through some simple examples that these correspond with the classical formulations found from Lagrange's equations. The dynamics is derived from the BoltzmannHamel equations of motion in terms of local position and velocity variables and the mapping to the corresponding quasi-velocities. Finally we present a new theorem which states that the BoltzmannHamel formulation of the dynamics is valid for all transformations with a Lie group topology. This has previously only been indicated through examples, but here we also present the formal proof.

Keywords:

Lagrangian mechanics, singularities, implementation, Lie theory

1 Introduction There are many motivations for deriving the explicit dynamic equations of mechanical systems. Firstly, the equations are needed for system and controller analysis.

by writing the conguration space in terms of abstract Lie groups. For a complete analysis, however, the explicit dynamics is required.

This is for example the

case when representation or implementation issues affect the stability of the mechanical system.

This analysis can to some extent be based on

Secondly, the dynamics is important in simulations

an abstract formulation of the dynamics, for example

and controller implementation. In this case it is of vital

doi:10.4173/mic.2012.2.2

c

2012 Norwegian Society of Automatic Control

Modeling, Identication and Control

importance that the obtained equations reproduce the

around one xed axis represented by the Schönies

actual behavior of the real system as closely as pos-

group

sible.

In this case we need the dynamics written ex-

plicitly in closed or recursive form. One major concern in this setting is to obtain a set of equations without the presence of representation singularities, which may arise when transforming an abstract formulation into the explicit dynamic equations. In an abstract formulation of the dynamics the representation singularities are not present simply because the representation, i.e., the choice of coordinates, have not yet been made. This is important to keep in mind when choosing the state variables of the system.

X.

Lie groups are the mathematical basis for several different approaches used to derive the dynamics of multibody systems (Selig, 2000; Park et al., 1995; Bullo and Lewis, 2000; Arnold, 1989; Bullo and Murray, 1999; Murray et al., 1994).

The state space is then repre-

sented by the tangent bundle, i.e., the position of the rigid body is described by an element of the Lie group, and the velocity state is described by an element of the Lie algebra. For this approach to be valid for all congurations, the conguration space needs to be covered by an atlas of local coordinate patches. This leads

We see that singularities may appear in the dynamics

to several dierent sets of equations, one for each co-

if an unfortunate set of variables is chosen to represent

ordinate patch. The appropriate equations must then

the state space. Examples of systems that often suer

be chosen for the current conguration. For example,

from singularities are ships and marine vehicles, space-

the geometric approach presented in Bullo and Lewis

craft and aircraft, robotic manipulators, and so on. It

(2000) can be used to obtain a globally valid set of

is, however, well known that if an appropriate set of

dynamic equations on a single Lie group, such as an

state variables is used to represent the state space of

underwater vehicle or spacecraft.

the system, the singularities are avoided.

The most

common way to do this is to use the quaternion representation to represent orientation or to represent the state space as the tangent bundle, i.e., the position as an element of the Lie group and the velocity variables as an element of the tangent space transformed by an action of the Lie group, i.e., an element of the Lie algebra.

Deriving the dynamics in this way does unfortunately require quite a few tools from dierential geometry and the nal equations are often written in a rather abstract form. This is particularly the case with multibody dynamics. The equations are abstract in the sense that the conguration space is given by "some matrix Lie group", often without specifying what the matrix looks like and how to represent it mathemat-

The quaternion representation is a popular represen-

ically.

Several of the properties that can be associ-

tation because we obtain a global representation of the

ated with the matrix depend on its representation and

attitude by using four variables to represent orienta-

this must therefore be taken into account when writ-

tion instead of the minimal representation with three

ing the equation explicitly. Similarly, the transforma-

variables (From et al., 2010).

Quaternions are there-

tions needed are represented by adjoint maps on the

fore both appropriate and a popular choice for repre-

Lie groups or Lie algebras, but it is not always straight

senting the rigid body orientation in kinematic equa-

forward to see what these transformations and their

tions. When we are to derive the dynamics, however,

derivatives look like.

the quaternions are not always suited because they are

such transformations in this paper.

not a minimal representation. The Lagrangian formal-

how we can use the structure of the Lie groups to sim-

ism, for example, requires a minimal representation of

plify the nal expressions.

the generalized coordinates of the system, which the quaternions are not. Even so, it is important to note that the quaternions can often be used in the implementation for attitude representation even though we often use more general formulations in the derivation.

We will see some examples of We will also see

One particular transformation for which it may be dicult to nd a mathematical representation is the mapping from local to global velocity variables: When taking a Lie theoretical approach to deriving the systems dynamics, the equations are derived on the tan-

Another solution that removes the singularities from

gent space. However, using our approach, the dynam-

the equations is to represent the state space as a tan-

ics is derived in terms of local velocity variables which

gent bundle. The position of the system is then repre-

are in general dierent from the twists. The transfor-

sented by an element of the Lie group, which for con-

mation from the local coordinates to the twists is thus

guration spaces of higher dimensions are represented

important because we want to write the nal equations

as matrix Lie groups. Common examples are free mo-

in terms of the globally dened twists, and not the lo-

SE(3),

cal state variables. In this way we derive the dynamics

tion represented by the Special Euclidean group

rotations around a point in space represented by the Special Orthogonal group resented by

46

SE(2),

SO(3),

in terms of the local variables in

Rm

to avoid singular-

planar motion rep-

ities, but we obtain a nal set of equations written in

and free translation and rotation

terms of the global velocity and position variables. We

From, Explicit Dynamics of Single Rigid Bodies

need both this mapping and its derivative to nd the explicit dynamic equations. The singularity-free dynamic equations of multibody systems were derived in Duindam and Stramigioli (2007, 2008) for mechanisms with general joints. The BoltzmannHamel formulation used allows us to derive the dynamics in terms of velocity variables

v

that are dierent from the time derivative of the position variables

q.

These velocity variables are nor-

mally referred to as quasi-coordinates. Duindam and Stramigioli (2008) showed that this transformation can be derived in terms of the local position variables of the system, which again allows us to nd a well dened velocity transformation matrix. In From et al. (2010) the same equations were used to derive the singularity-free dynamic equations of vehicle-manipulator systems. In From et al. (2012), a few mistakes in the above mentioned papers were corrected and the correct dynamic equations were presented.

The dynamic equations for the most important Lie groups are found in Section 4 and compared to the

In this paper we will de-

rive in detail the explicit dynamic equations based on the correct formulation rst presented in brief in From et al. (2012).

standard formulation. It is however also important to show that the equations found are general. In Section 5 we therefore nd a general theorem valid for all conguration spaces with a Lie group topology. We show for the rst time that the BoltzmannHamel equations derived from exponential coordinates rst presented in From et al. (2012) are in fact valid for all Lie groups. Even though the derivation of the dynamic equations is not standard, the nal expressions that we arrive at are well known from literature. It is, however, important to derive these simple dynamic equations because the formulation used allows us to combine these into the dynamics of multibody systems. The expressions presented in this paper can therefore be used as a basis for implementing more complex systems. For example, for a multibody system we can consider the expressions presented in this paper a list of available conguration spaces, and we can simply pick the correct conguration space for each transformation in the multibody system. In fact, one of the reasons that we derive the dynamics in terms of quasi-velocities in this way is that

The nal expressions written in terms of the global

we are then able to automatically generate multibody

variables are rather simple but the derivations and

dynamics based on the properties of the conguration

computations are somewhat complex, and it is there-

space of each joint or transformation looked at sepa-

fore important to show that the formulation of the

rately.

BoltzmannHamel equations of motions presented in

mechanics of a general multibody mechanism we can

From et al. (2012) is in fact correct. We will therefore

use the results obtained for single rigid bodies directly

derive the explicit dynamic equations for several dier-

because the velocity transformation matrices can be

ent conguration spaces and show that, when reduced

combined into one big block-diagonal velocity trans-

to single rigid body dynamics, these are equivalent to

formation matrix representing the whole system. Also,

the standard formulation for single rigid bodies found

deriving the dynamics in terms of local coordinates al-

in the classical literature, such as Euler's equations and

lows us to write the dynamics without the presence of

NewtonEuler equations of motion. It is an important

singularities also when the conguration space is not

principle that even though there are many dierent ap-

Euclidean.

Moreover, we will see that when deriving the

proaches to deriving the dynamic equations, we should arrive at the same nal equations when written out explicitly. If we were to nd the explicit expressions for the dynamics based on the formulation in Duindam

2 Rigid Body Dynamics

and Stramigioli (2007, 2008), for example, we would

In this section we present the dynamic equations of a

nd that these are dierent from the standard formu-

single rigid body. We derive the dynamics from the La-

lation of the dynamics and, more importantly, nor can

grangian in the classical way which results in the well

they be reformulated into these.

known dynamic equations of single rigid bodies, such as

The equations pre-

R3

sented in Duindam and Stramigioli (2007, 2008) and

Euler's equations for rotational motion in

From et al. (2010) are therefore wrong, which is not

EulerLagrange equations for free motion in

and the

easy to see without comparing them to the standard

will use these equations to show that the equations that

formulation and certainly not easy to see from the

we obtain from the BoltzmannHamel formulation in

derivation.

We use this as a motivating example to

Section 3 are identical to the Euler and NewtonEuler

show the importance of writing out the equations ex-

equations found in the classical literature on mechani-

plicitly, especially in the case when the derivations are

cal systems (Goldstein et al., 2001; Arnold, 1989).

R3 .

We

complex and lengthy. We show that using the formula-

In Section 2.3 we present the dynamic equations as

tion presented in this paper we arrive at the dynamic

presented in Duindam and Stramigioli (2008) and show

equations traditionally presented in literature to calcu-

that these do not correspond with the dynamic equa-

late the dynamics of these systems.

tions found in the classical literature.

We start with

47

Modeling, Identication and Control

a short overview of the dynamics of single rigid bodies for the most important conguration spaces.

derived using the Lagrangian framework. Considering kinetic energy only this will lead to the dynamic equations

B B B M ω˙ 0b = (M ω0b ) × ω0b

(1)

inertia matrix of the rigid body and

B ω0b

T

we get the well known

M

B B B M V˙ 0b + C(V0b )V0b =τ B B M V˙ 0b − adT V M V0b = τ

is the

is the angular

Fb with respect to the F0 as seen from Fb . We can further allow external forces τ to act on the rigid body and write the velocity of the rigid body frame



(9)

(10)

These equations can also be written as

These equations are

known as Euler's equations of motion. Here

τωT

   ∂K ∂K B +ω ˆ 0b = τv ∂v B ∂v B    0b   0b  ∂K ∂K ∂K d B B + vˆ0b +ω ˆ 0b = τω . B B B dt ∂ω0b ∂v0b ∂ω0b d dt

The dynamic equations of a single rigid body can be

SO(3).

 τ = τvT

Kirchho 's equations of rigid body motion

2.1 Motion of Single Rigid Bodies

for a conguration space

If we write

with

adV

(11)

as in (8).

inertial frame

equations explicitly as

where

2.2 The BoltzmannHamel Equations of Motion

Ix p˙ − (Iy − Iz )qr = τp

(2)

Iy q˙ − (Iz − Ix )rp = τq

(3)

a rigid body allows us to write the velocity state in

Iz r˙ − (Ix − Iy )pq = τr

(4)

terms of quasi-velocities. Quasi-velocities are velocity

 B ω0b = p

q

r

T

inertia matrix is written

 T τ = τp τq τr and the as M = diag(Ix , Iy , Iz ) which

,

is possible if we choose the body frame so that its origin

The BoltzmannHamel formulation of the dynamics of

variables

v

that are not necessarily the time derivative

of the position variables

These

equations can then be written as

v = S(q)q. ˙

3×3

pˆ ∈ R pˆx = p × x

where that

+

B B ω ˆ 0b M ω0b

for all

p, x ∈ R



B ω ˆ 0b

this type of expressions, for example in velocity trans-

=τ

(5)

0 = r −q

. In this case we have

 q −p . 0

−r 0 p

where

Euler angles and the body velocity variables. The Lagrangian can be written in terms of the gen-

∂K B ∂V0b



− adT V



eralized coordinates

(6)

SE(3)

the La-

grangian formalism will result in the equations



formation matrices relating the time derivative of the

body frame

Similarly, for the conguration space

d dt

Because both the velocity and position variables are

is the skew-symmetric matrix such

3

(12)

written in vector form, singularities tend to arise in

B B B M ω˙ 0b + C(ω0b )ω0b =τ B M ω˙ 0b

They are related to the time

in the form

is in the center of gravity of the rigid body and its axes are aligned with the principal axes of inertia.

q.

derivative of the position variables by a linear relation

∂K B ∂V0b

1 T v M v − U(x). 2

(13)

The partial derivatives of the Lagrangian in (13) are given explicitly as

∂L = M v,  ∂v  d ∂L = M v, ˙ dt ∂v ∂L ∂U(x) =− . ∂x ∂x

(7)

 B T T (ω0b ) and h B Bi ω ˆ v ˆ adV = 00b ωˆ0b . B

x and the velocity variable v in the

as

L(x, v) =

 =τ

Fb ,

 B T B ) V0b = (v0b

(8)

(14)

(15)

(16)

0b

These

equations

are

normally

referred

NewtonEuler equations of motion.

adω = ω ˆ the SO(3) by using

to

as

the

Note that if we

write

formulation in (7) reduces to (5)

for

that

ω ˆ T = −ˆ ω.

In this paper we

will for the rst time show that this is in fact a general result.

48

We can also write the velocity

v

in terms of the time

derivative of the generalized velocity as

v = S(x)x˙ .

This allows us to write the Lagrangian as a function of generalized coordinates as

x

and generalized velocities

1 ¯ x) L(x, ˙ = x˙ T S(x)T M S(x)x˙ − U(x) 2

x˙

(17)

From, Explicit Dynamics of Single Rigid Bodies

Proof.

which can be substituted into Lagrange's equations

for some

B(x)

(18)

yet to be determined. We nd the par-

tial derivatives as

S −T (x).

We can simplify these equations by writing

where



∂L ∂v

γk



∂L ∂v  ¯   ∂L d ∂L d ∂L T T ˙ = S (x) + S (x) dt ∂ x˙ ∂v dt ∂v T ¯ ∂L ∂ (S(x)x) ˙ ∂U = M S(x)x˙ − ∂x ∂x ∂x T ∂ (S(x)x) ˙ ∂U = Mv − |{z} ∂x ∂x} | {z = S T (x)

is a function of

X

γk v k

k

S(q).

∂L =τ ∂v

(27)

The formulation of

in Duindam and Stramigioli (2008), however the nal (19)

expression for the Coriolis matrix was not presented correctly.

(20)

One of the objectives of this paper is to

show how to arrive at the correct explicit expression for Coriolis matrix from the formulation of the dynamics given in (27). To avoid singularities it is possible to use local position and velocity variables on this topic.

and

ϕ˙ ∈ Rm .

In the context of this paper is suces

to know that we can nd the mapping from the time (21)

derivative of the local position coordinate to the body velocities

The EulerLagrange equations are found by the par-

¯ x) L(x, ˙

ϕ ∈ Rm

We refer to Duindam and Stramigioli (2008) for details

∂L ∂x

∂L ∂ T (S(x)x) ˙ ∂L + . ∂x ∂x ∂v

tial derivatives of the Lagrangian

!

∂L − S −T + ∂q

the dynamics shown in (26) was correctly presented

∂L ∂v

=

multiplying with

d dt

¯ ∂L = S T (x)M S(x)x˙ ∂ x˙ = S T (x) |{z} Mv

∂L ∂v

The proof follows directly by substituting the

expressions in Equations (19-21) into (22) and pre-

 ¯ ¯ d ∂L ∂L − = B(x)τ dt ∂ x˙ ∂x

v = S(ϕ)ϕ˙

as

 ¯ ¯ ∂L ∂L − = B(x)τ ∂ x˙ ∂x   d ∂L ∂L S T (x) + S˙ T (x) dt ∂v ∂v d dt

(22)

(28)

in terms of the local position variables as (Rossmann, 2002)

 S(ϕ) =

1 1 I − adϕ + ad2ϕ − . . . 2 6



∈ Rm×m

(29)

T

−

˙ ∂L ∂L ∂ (S(x)x) − = B(x)τ ∂x ∂x ∂v

(23)

and can thus be written in terms of the Lagrangian The torques with

v

τ

L.

are dened so that they are collocated

(represented in the body frame) and we can

therefore write the work

W

as

W = v T τ = (S(x)x) ˙ T τ = x˙ T S T (x)τ

(24)

B(x) = S T (x) as expected. We therefore −T pre-multiply (23) with S (x) to get the dynamics in which gives

the right form:

where

adϕ

is the matrix representation of the Lie

bracket, which for

SE(3)

is shown in (8).

The dynamics in terms of the local parameterization is found in the same way as above with

d dt

 ¯    d ∂L ∂ Lϕ ∂L T T ˙ + S (Q, ϕ) = S (Q, ϕ) ∂ ϕ˙ ∂v dt ∂v (30)

¯ϕ ∂L ∂L ∂ T (S(Q, ϕ)ϕ) ˙ ∂L = + . ∂ϕ ∂ϕ ∂ϕ ∂v

(31)

Proposition 2.1. For a mechanical system with La- 2.3 The Coriolis Matrix as Shown in grangian

1 ¯ x) L(x, ˙ = x˙ T S(x)T M S(x)x˙ − U(x) 2

Duindam and Stramigioli (2008)

(25)

we can nd the dynamic equations in terms of the Lagrangian L(x, v) in (13) with v = S(x)x˙ as d dt



∂L ∂v

in the velocity transformation matrix

when the Euler

and Stramigioli (2008) introduce local variables

ϕ˙

for velocity.

found by substituting the equations at

v = S(ϕ)ϕ˙

tion variables.

ϕ=0

ϕ

for

The dynamics are then into the expressions

above, dierentiating with respect to (26)

S

angles are used to represent the orientation, Duindam position and



∂L − S −T (x) ∂x   ∂ T (S(x)x) ˙ ∂L −T T ˙ + S (x) S (x) − = τ. ∂x ∂v

In order to avoid the singularities that normally arise

ϕ, and evaluating

to obtain the global congura-

We refer to Duindam and Stramigioli

49

Modeling, Identication and Control

(2008) for more details on this topic.

The dynamics

can now be written as

M (Q)v˙ + C(Q, v)v = τ Q

where

(32)

is the matrix representation of the congu-

ration space, for example a matrix representation of

SO(3)

or

SE(3),

and

C(Q, v)

3 Derivation of the Coriolis Matrix

is the matrix describing

the Coriolis and centrifugal forces. The Coriolis matrix is given in (Duindam and Stramigioli, 2008, Eq. (26)) as

In this section we derive the explicit dynamic equations for a single rigid body from the BoltzmannHamel equations of motion.

A short version of the results

presented here is given in From et al. (2012), but in this section we also present some more details in the derivation and show how we arrive at the nal equations.

Cij (Q, v) =

X 



−1 Smi

k,l,m,s

∂Sjs ∂Sjm − ∂ϕs ∂ϕm



 −1 Ssk Mkl vl .

Following the notation in Duindam and Stramigioli (2007, 2008) we will write the dynamics as

ϕ=0

(33)

d dt

We will now look at what the dynamics will look like for a single rigid body with conguration space



∂L ∂v

 −S

!

−T ∂L

X

+

∂q

γk v k

k

SE(3)

∂L =τ ∂v

(38)

with the formulation above. We rst nd an expression for

S(ϕ),

which for a single rigid body can be written

γk .

as (Rossmann, 2002)

 S(ϕ) =

so it only remains to nd an expression for the matrix

1 1 I − adϕ + ad2ϕ − . . . 2 6



∈ Rm×m

(34)

Comparing (38) with (26) gives

X

γ k vk =

X

k

adX

where

X

ϕ

respect to

m.

Since we are to dierentiate with

and substitute

ϕ = 0

k

Cij (v) =

k

 ∂Sji ∂Sjk − ∂ϕk ∂ϕi

We will rst look at

(M v)k .

(35)

matrix

o ij

are equal to

with 1 in the

n

∂Sjk ∂ϕi

o

by the

ij

k th

k

γk,1 vk = S −T (q)S˙ T (q).

The

can be written as

 −1 S11 −1 S12  −T S (q) =  .  .. −1 S1n

− 12 adT ek

ϕ = 0 ek

where

the matrices is a 6-vector

entry and zeros elsewhere. Similarly,

is equal to

k th

S −T (q)

P

ϕ=0

dierentiating and evaluating at

∂Sji ∂ϕk

(39)

we see from (34)

From the mapping in (34) we also note that after

n

γk,2 vk

k

that we can simplify (33) slightly to

X

X

∂ T (S(q)q) ˙ . = S −T (q)S˙ T (q) − S −T (q) ∂q

is the adjoint map for a general Lie algebra

of dimension

γk,1 vk +

1 2

adT ek .

This is then multiplied

element of the vector

Mv

··· ···

. . .

..

−1 S2n

...

−1  Sn1 −1  Sn2  .  .  . −1 Snn

.

(40)

−1 Sij is to be interpreted as element (i, j) of the −1 matrix S (not the inverse of the element Sij ). The

where and the Coriolis

matrix is therefore given by

C(v) =

−1 S21 −1 S22

− adT (M v)

transpose of the time derivative of the velocity trans(36)

which gives the following dynamics:

M v˙ + C(v)v = τ M v˙ − adT (M v) v = τ.

(37)

We note that this is not the same as the NewtonEuler

formation matrix, i.e.,

 P ∂S 11 m ∂q q˙m  P ∂Sm 12 q˙m  m ∂qm S˙ T =  .  .  . P ∂S1n m ∂qm q˙m

S˙ T (q),

is given by

∂S21 ∂qm q˙m ∂S22 m ∂qm q˙m . . . P ∂S2n m ∂qm q˙m

P Pm

··· ... ..

.

...



∂Sn1 ∂qm q˙m  ∂Sn2 m ∂qm q˙m   .  .  . P ∂Snn q ˙ m ∂qm m

P Pm

(41)

equations in (11), nor can they be re-arranged into these.

We can therefore conclude that the formula-

q˙ = S −1 (q)v

P

We now rewrite

explicit expressions of the Coriolis matrix.

which allows us to write the rst part of

50

as

q˙m =

tion in (33) is wrong. We will now present the correct

−1 Smk (q)vk γk vk , i.e.,

k

From, Explicit Dynamics of Single Rigid Bodies

γk,1 vk = S −T (q)S˙ T (q)

P

k

as

local variables are always Euclidean) we can also write this as

X X X ∂Sjl (γk,1 )ij vk = Sli−1 q˙m ∂qm m k l X ∂Sjl q˙m = Sli−1 ∂qm

(γk )ij (q) =

−S −T (q) ∂

T

second

(42)

Pγk . k γk,2 vk

part

∂(S q) ˙ 1 ∂q2 ∂(S q) ˙ 2 ∂q2 . . . ∂(S q) ˙ n ∂q2

∂q1

 ∂(S q) ˙  ∂q1 2 ∂(S(q)q) ˙ =  .. ∂q  .

∂(S q) ˙ n ∂q1

and then if we write write the matrix

P m P  m    P

∂S1m ∂q1 q˙m ∂S1m ∂q2 q˙m . . . ∂S1m m ∂qn q˙m

··· ... ..

(S(q)q) ˙ l =

.

... P

∂(S q) ˙ 1 ∂qn ∂(S q) ˙ 2 ∂qn   .  .  . ∂(S q) ˙ n ∂qn

m Slm q˙m

∂ T (S(q)q) ˙ as ∂q

(47)

The following proposition was rst

Proposition 3.1. Consider a single rigid body with lo-

cal position and velocity coordinates ϕ and ϕ˙ and global position and velocity coordinates Q and v . Write the kinetic energy as K(v) = 12 v T M v with the inertia matrix M . The dynamics of this system then satises

=

(S(q)q) ˙ we rst write ∂q

 ∂(S q) ˙ 1

−1 Smk

present the detailed proof.

from the rst part of the



∂Sjm ∂Sjl − ∂ϕm ∂ϕl

presented correctly in From et al. (2012), but here we

(γk,1 )ij

q˙

ϕ.

cal variables

∂Sjl X −1 = Sli−1 Smk vk ∂qm l,m k   X X ∂Sjl −1   vk = Sli−1 S ∂qm mk k l,m {z } |

for



and we can derive the dynamics in terms of the lo-

l,m

Similarly

Sli−1

l,m

X

which eliminates

X

(43)

M v˙ + C(v)v = τ

(48)

where M is found in the normal way, with τ the vector of external and control wrenches (collocated with v ), and the matrix describing the Coriolis and centrifugal forces given by Cij (ϕ, v) =

we can

X

Sli−1



k,l,m,s

∂Ssl ∂Ssj − ∂ϕm ∂ϕl



−1 Smj Msk

vk . ϕ=0 (49)

∂S2m ∂q1 q˙m ∂S2m m ∂q2 q˙m . . . P ∂S2m m ∂qn q˙m

P Pm

..



∂Snm ∂q1 q˙m  ∂Snm m ∂q2 q˙m  . .  .  . P ∂Snm q ˙ m m ∂qn

P Pm

··· ··· .

···

For a single rigid body this becomes Cij (ϕ, v) =

X

Sli−1



l,m,s

∂Ssj ∂Ssl − ∂ϕm ∂ϕl



−1 Smj

(M v)s . ϕ=0 (50)

(44) We nd an expression for

X k

(γk,2 )ij vk = −

X

P

Sli−1

k

γk,2 vk

To compute the matrix C(v) for a single rigid body with conguration space SE(3) or one of its subgroups, we can use the structure of the Lie algebra to simplify (50) slightly to

as:

X ∂Sjm

q˙m ∂ql X ∂Sjm X −1 =− Sli−1 Smk vk ∂ql k l,m   X X ∂S jm  S −1  vk . =− Sli−1 ∂ql mk k l,m | {z } m

l

Cij (ϕ, v) =

∂Skj − ∂ϕj ∂ϕi

k (45)

Proof.

Cij (ϕ, v) =

X X s

We can now nd

=

(46)

γ k vk

k

X

Sli−1

s,l,m,k

l,m



(M v)k .

(51)

ϕ=0

From Equation (38) and (47) we rst nd

!

(γk,2 )ij

γk = γk,1 + γk,2 as   X ∂Sjl ∂Sjm −1 −1 (γk )ij (q) = Sli − Smk . ∂qm ∂ql

X  ∂Ski

is



Msj

(52)

ϕ=0

∂Ssm ∂Ssl − ∂ϕm ∂ϕl



−1 Smk

vk Msj . ϕ=0

We can use these new expressions to write the dynam-

We know that there are many dierent ways to write

ics in terms of local coordinates. Because we can sub-

the Coriolis matrix which all result in the same vec-

stitute the variable vector

q

with local variables (the

tor

C(ϕ, v)v .

We denote this vector

C(ϕ, v)v =

51

Modeling, Identication and Control

T . . . cn and X ci (ϕ, v) = Cip vp

 c1

c2

which after a change in the index name becomes

write

Cij (v) =

p

! =

X X X p

s

Msp

=

p

s

γ k vk

=

s

γ k vk

the position variables

Msp vp

state matrix

XX s

γ k vk

k

=

XX s

Sli−1



Sli−1



l,m,k

 =

X

X 

l,m,s

k

vanish from the equations and

Q.

Admittedly, the formulation is rather

we use the expressions for the velocity transformation

(M v)s is



ϕ

v. ϕ=0

and

complex, but we will see in the next section that when

! =

ϕ

the position is written in terms of the conguration

p

is

ϕ=0

However, when we evaluate the expressions av

is

k

(57)

We see that we have found a formulation of the dy-

! X

(M v)k .

namic equations in terms of the variables

Msp vp

k

XX



vp

is

! XX X

∂Skj − ∂ϕj ∂ϕi

k

!

γ k vk

k

X  ∂Ski

∂Ssl ∂ϕm ∂Ssl ∂ϕm

|

matrices S(ϕ) for the dierent conguration spaces,  the nal expressions will take a very simple form. ∂Ssm −1  vk (M v)s − Smk ∂ϕl ϕ=0   ∂Ssm −1 − Smk (M v)s  vk . ∂ϕl The kinematics of the system can be naturally deϕ=0 {z } scribed in terms of the state variables g0b for posiCik B tion/orientation and V0b for velocity. To allow for more (53)



4 The Most Important Conguration Spaces

general systems with other conguration spaces than

M does not SE(3), we will write the conguration of a rigid body depend on ϕ so there is no need to evaluate M at ϕ = 0. as a matrix Lie group Q of arbitrary but constant dim Because ci = Cij vj a change in the summation indexes mension. The velocity variable is written as v ∈ R gives the Coriolis matrix for a conguration space of dimension m ≤ 6. Using this formalism we obtain a global parameterization of   X ∂Ssl ∂Ssm a rigid idealized joint (Duindam, 2006): −1 −1 Cij (v) = Sli − Smj (M v)s . ∂ϕm ∂ϕl Here we have used the observation that

l,m,s

Denition 4.1. A globally parametrized rigid trans-

ϕ=0

(54) Now, recall that the velocity transformation matrix can be written as

 S(ϕ) = where

X

adX

1 1 I − adϕ + ad2ϕ − . . . 2 6



∈ Rm×m

(55)

is the adjoint map for a general Lie algebra

m. Because the expression is to be at ϕ = 0 this expression is non-zero only for m = j , see (55). The nal expressions then

of dimension

evaluated

l=i

and

• a matrix Q, parameterizing the relative conguration as gij = gij (Q); and • a vector v ∈ Rm , parameterizing the relative twists as VijB = Hv . For convenience, both for representation and for implementation, we will normally write the conguration

become

−1 Cij (v) = Sli−1 Smj (M v)s l,m,s ϕ=0   X ∂Ssi ∂Ssj −1 −1 = Sii − Sjj (M v)s ∂ϕj ∂ϕi s ϕ=0  X  ∂Ssi ∂Ssj = − (M v)s (56) ∂ϕj ∂ϕi X

s

52

formation is a kinematic restriction on the allowed relative twist of two rigid bodies i and j to a linear subspace of dimension m, where the relative motion of the bodies is described by two sets of states, namely



∂Ssl ∂Ssm − ∂ϕm ∂ϕl

ϕ=0



as a homogeneous transformation matrix and the velocity as a twist. Because all conguration spaces that are of interest to us are subgroups of

SE(3), we can al-

ways write the state space as an element of the tangent bundle in this way. We see that it is important that we adapt a formalism that satises the restrictions of the conguration space also when the conguration space has less than six degrees of freedom.

For rigid bodies with a con-

guration space other than

SE(3)

and with dimension

From, Explicit Dynamics of Single Rigid Bodies

m