Chapter 2

Modeling of Rigid Mechanisms

The goal of this chapter and the next is to obtain building blocks that can be used
to construct mathematical models of the mechanical behavior of general walk-
ing robots. This chapter contains the necessary ingredients for the description of
mechanisms comprised of a finite number of rigid links interconnected by ideal
joints, possibly moving freely in space. Chapter 3 discusses models for collisions
and contact of rigid mechanisms with their environment.
A rigid link (or rigid body) is defined as a finite volume of point masses, all of
which have fixed relative distances. The most common example is a piece of solid
material (e.g. aluminum) in which the point masses are the atoms of the material
with constrained relative distances due to the structure of the solid.
An ideal joint is defined as a constraint between two rigid links that allows
only certain relative velocities and prevents others, independently of the forces
and torques applied to the links. An example is an (ideal) door hinge, which
constrains the velocity of the door to be a rotation around the vertical hinge axis,
relative to the building, even though gravitational forces may try to translate it
vertically.
These definitions show that rigid links and ideal joints are idealizations of
practical objects. Real links always have a certain stiffness, i.e. with a large
enough force or a large enough operating frequency they will bend considerably
or even break. Similarly, when real joints are subjected to large enough forces in
the constrained directions, they will break. For the purpose of practical walking
robots, however, the assumption of rigidity of links and idealness of joints is rea-
sonable: robot links and joints are designed to be stiff (rigid) and strong (ideal) to
increase their lifetime, and also to simplify modeling and control of these robots,
which is much harder if the links are flexible. The frequencies of the motion and
magnitudes of the forces are low enough to allow this design. However, as in all
modeling tasks, models should be tested against the real practical realizations to
check if the modeling assumptions are justified.

15
16 CHAPTER 2. MODELING OF RIGID MECHANISMS

In this chapter, we develop a dynamic model of a general rigid mechanism

in several steps. We first look at the description and analysis of the kinematics of
rigid mechanisms (Sections 2.1 and 2.2), then at the description of the dynamics of
the mechanisms in the framework of port-Hamiltonian systems (Section 2.3), and
finally at methods to deal with kinematic loops and nonholonomic constraints
(Section 2.4).

2.1 Kinematics of rigid bodies

The objective of a kinematic study is to obtain insight in the possible configura-
tions and velocities that a mechanical system can have, without looking at the
physical causes for these configurations and velocities. In other words, we study
and try to describe the possible motions of a mechanical system without looking
at the forces acting on it.

2.1.1 Configuration of a rigid body

As a first step, we consider a single rigid body, freely moving in space. The con-
figuration of this rigid body could be described by the positions of all the point
masses that are part of the body. If, for example, we consider a one kilogram
slab of aluminum, we could describe its configuration (expressed in some coordi-
nate frame) by the positions of all the atoms; this would amount to roughly 1026
variables.
Fortunately, the assumption of rigidity can reduce the dimension of the state
space enormously. The point masses are constrained to have certain fixed relative
distances, and so the positions of three fixed non-collinear points of the body are
enough to determine the positions of all the other points. To be more precise,
we prove the following lemma, using the definitions of the various spaces from
Appendix A.3.

Lemma 2.1. The space of all possible configurations of a rigid body in three-
dimensional space, relative to some reference frame, is the six-dimensional space
SE(3), which is topologically equivalent to the set R1 × R1 × R1 × S2 × S1 .

Proof. We prove the lemma geometrically by considering Figure 2.1. Suppose

we have a rigid object with three non-collinear reference points on it. How many
degrees of freedom do we have to place this object in a three-dimensional world?
Starting in Figure 2.1a, we see that the first reference point (labeled p1 ) can be po-
sitioned freely in space, i.e. there are three degrees of freedom for this point. Then,
following Figure 2.1b, there are only two degrees of freedom in positioning the
second reference point p2 , since the rigidity constraint forces p2 to be somewhere
on a sphere with center p1 and radius r1→2 (ra→b denotes the distance between pa
2.1. KINEMATICS OF RIGID BODIES 17

z z z

x y y r2→3 y
p1 p2 p1 p2 p1
r1→2 p3
r1→3

(a) Three degrees of freedom (b) Two degrees of freedom (c) One degree of freedom for
for the first point. for the second point. the third point.

Figure 2.1: Three non-collinear points on a rigid body determine its configuration.

and pb ). Finally, Figure 2.1c shows that the last reference point p3 must be both on
a sphere centered around p1 and on a sphere centered around p2 , i.e. , on the in-
tersection of the two spheres: a circle. The position on this circle has one degree of
freedom, making a total of six degrees of freedom. With all three reference points
positioned, the object can be constructed using the other rigidity constraints.
The resulting space is thus indeed the six-dimensional space R1 ×R1 ×R1 ×S2 ×
S1 : an element of R1 × R1 × R1 to fix the position of p1 , subsequently an element
of S2 to fix the position of p2 , and finally an element of S1 to fix the position of
p3 and hence the whole body. Grouping together the translation parts and the
rotation parts, we obtain the equivalent group T (3) × SO(3). T (3) is the group
of translations in three dimensions, and SO(3) is the special orthogonal group in
three dimensions, i.e. the group of 3D rotations. The space S2 × S1 is called the
Poincare sphere and is a representation of SO(3), as shown in Marsden & Ratiu
(1999).
Combining the two resulting groups T (3) and SO(3), we obtain the group
known as SE(3), the special Euclidean group in three dimensions, which hence
is the space of all possible 3D configurations of a rigid body, relative to a certain
frame. 
In numerical computations and simulations, instead of describing the relative
configuration of a rigid body as an abstract element of SE(3), we would like to
use coordinates, i.e. real numbers. Since SE(3) is a six-dimensional space, we
would like to use six real numbers to describe it, just like we would to describe
the six-dimensional Euclidean space.
Unfortunately, SE(3) is topologically different from the Euclidean space, and
it is hence not possible to continuously and globally cover it using six coordinates.
Instead, several representation methods exist to describe SE(3) either only locally
continuously using six numbers, or globally continuously using more than six
numbers. Selig (2005) discusses representation theory in the context of robotics.
Examples of locally continuous descriptions are the often-used methods of
Euler angles. In these methods, the rotation part is parameterized by three con-
18 CHAPTER 2. MODELING OF RIGID MECHANISMS

secutive rotations about local axes (which axes depends on the precise variation
of the method). The amounts of rotation provide three numbers (angles), which,
together with three coordinates for translation, give a parameterization of SE(3).
For every element of SE(3) there exist such coordinates, so the covering is global.
However, whatever variation is chosen, there are always rotations that are de-
scribed by only two coordinates with the third arbitrary (when using consecutive
rotations around x, y, and z, for example, the coordinates (a − θ, π2 , θ) describe
the same rotation for all θ). The result of this is that, during a smooth, continuous
change of rotation, the coordinates can have non-smooth discontinuities! Since
we want to take time-derivatives of rotations, these artifacts are undesired.
An example of a globally
 continuous but redundant representation  is by a
vector of the form q = cos( θ2 ), n1 sin( θ2 ), n2 sin( θ2 ), n3 sin( θ2 ) with n21 +n22 +n23 = 1.
The three numbers ni define the unit axis of rotation in three-dimensional space,
and the number θ the angle of rotation around this axis. The vector q is usually
thought of as a unit quaternion (Selig 2005). Together with three numbers for
translation, this quaternion can be used as a representation of SE(3). However,
the quaternion q is constrained to have unit norm, and there is a double covering
of the space of rotations (a 360◦ rotation around a certain axis is the same as a
0◦ around that axis, but the quaternion representation for these two rotations is
different).
In this thesis, we choose to use a representation method that uses so-called
homogeneous matrices to describe the relative configuration (translation and ro-
tation) of two coordinate frames in three-dimensional space. Homogeneous ma-
trices have the advantage of being globally continuous and being easy to perform
calculations with, i.e. using basic matrix multiplications. Homogeneous matrices
are defined as follows.
Definition 2.2 (Homogeneous Matrices). A homogeneous matrix H is a matrix
of the form
⎡ ⎤
..
Rxx Rxy Rxz . px ⎥
  ⎢
⎢ . ⎥
R p ⎢Ryx Ryy Ryz .. py ⎥
H := =⎢ .. ⎥ ∈ R4×4 (2.1)
0 1 ⎢Rzx Rzy Rzz . pz ⎥
⎣ . . . . . . . . . . . . . . . . . . . . . . .⎦
..
0 0 0 . 1

with R−1 = RT ∈ R3×3 , det(R) = 1, and p ∈ R3×1 .

Using the same notation as Stramigioli (2001), we can use homogeneous matrices
in the following way to describe the configuration (position and orientation) of a
rigid body.
1. Choose a right-handed coordinate frame Ψi as a reference frame, in which
the configuration of the rigid body will be expressed;
2.1. KINEMATICS OF RIGID BODIES 19

2. Rigidly attach another right-handed coordinate-frame Ψj to the body, where

‘rigidly attaching’ means that the coordinates of all points of the body, when
expressed in Ψj , are constant in time, independently of the motion of the
body;
3. Define pij as the 3 × 1 column vector of coordinates of the origin of Ψj ex-
pressed in Ψi ;
4. Define Rji as the 3 × 3 matrix with the columns equal to the coordinates of
the three unit vectors along the frame axes of Ψj expressed in Ψi ;
5. Construct the matrix Hji using (2.1) with R = Rji and p = pij .

We first prove that this definition of Hji gives a proper homogeneous matrix, i.e.
that it satisfies the required properties on R. Let us denote by â the unit vector in
the direction of coordinate axis a and compute
⎡ T⎤ ⎡ T ⎤ ⎡ ⎤
x̂   x̂ x̂ x̂T ŷ x̂T ẑ 1 0 0
(Rji )T Rji = ⎣ŷ T ⎦ x̂ ŷ ẑ = ⎣ŷ T x̂ ŷ T ŷ ŷ T ẑ ⎦ = ⎣0 1 0⎦ (2.2)
ẑ T ẑ T x̂ ẑ T ŷ ẑ T ẑ 0 0 1

where the last step follows since the axes of a coordinate frame are orthogonal to
each other and the vectors x̂, ŷ, and ẑ have unit length. This proves that (Rji )T =
(Rji )−1 . Now, to prove det(Rji ) = 1, we compute
 
det(Rji ) = det( x̂ ŷ ẑ ) = x̂ × ŷ, ẑ = 1 (2.3)

where the last step follows since the vectors have unit length and since the frame
is right-handed, hence the cross product of x̂ and ŷ by definition equals ẑ. So
indeed, the matrix Hji defined in this way is a proper homogeneous matrix of the
form (2.1).
However, although the matrix may be homogeneous, it remains to be shown
how it describes the configuration of the rigid body, which was its purpose. To
this end, we need the following definitions of the augmented coordinates of a
(free) vector and of a point.

Definition 2.3 (Coordinates of a Vector). The (augmented) coordinate vector V i

corresponding to a (free) vector v in space is the vector
⎡ i⎤
 i xv
v ⎢ yvi ⎥
V i := := ⎢ ⎥
⎣ zvi ⎦ ∈ R
4
(2.4)
0
0

with xiv , yvi , zvi the orthogonal projections of v on the respective axes of a right-handed
coordinate frame Ψi , i.e. the conventional coordinates v i of the vector v.
20 CHAPTER 2. MODELING OF RIGID MECHANISMS

Definition 2.4 (Coordinates of a Point). The (augmented) coordinate vector Qi

corresponding to a point q in space is the vector
⎡ i⎤
 i xq
q ⎢ yqi ⎥
Qi := := ⎢
⎣
⎥ ∈ R4 (2.5)
1 zqi ⎦
1

with xiq , yqi , zqi the coordinates of the vector from the origin of the right-handed frame
Ψi to the point q.

The coordinates of a single point q, expressed in different coordinate frames Ψi

and Ψj , are related by the homogeneous matrix Hji as constructed before, namely
in the following way.

Qi = Hji Qj (2.6)

This can be proved as follows: for a point q with coordinates Qj when expressed
in Ψj , its coordinate vector q i is the vector from the origin of Ψi to the origin
of Ψj plus the vector from the origin of Ψj to the point q expressed in Ψi , or
mathematically
   
q i = pij + x̂i x̂Ti x̂j + ŷi ŷiT x̂j + ẑi ẑiT x̂j qxj + x̂i x̂Ti ŷj + ŷi ŷiT ŷj + ẑi ẑiT ŷj qyj
 
+ x̂i x̂Ti ẑj + ŷi ŷiT ẑj + ẑi ẑiT ẑj qzj = pij + Rji q j (2.7)

which can be written conveniently as the matrix multiplication (2.6).

When the coordinates Qi of a point q are expressed in a frame Ψi that is rigidly
attached to the body to which also q is rigidly attached, these coordinates are
often called body-coordinates. Similarly, when the coordinates Q0 are expressed in
a frame Ψ0 that is considered a reference frame, the coordinates are called world-
coordinates.
The property of defining a coordinate transformation by a simple matrix mul-
tiplication can be used for various purposes. Knowing the relative homogeneous
matrix between two coordinate frames means that quantities computed numer-
ically in one of these frames can be transformed to the other frame by a matrix
multiplication. Furthermore, when rigid bodies are moving around over time,
we can describe this motion by a time-varying homogeneous matrix, and use it
to describe the time-evolution of the position of any point attached to the body
by a multiplication of the body-coordinates of that point. These properties show
that indeed, the homogeneous matrix, constructed as before, describes the con-
figuration (position and orientation) of a rigid body.
Homogeneous matrices have another nice property, which follows from the
interpretation as a coordinate transformation. Since coordinate transformations
2.1. KINEMATICS OF RIGID BODIES 21

θ
z

z Ψi
y
x
b
x
Ψ0 y a

Figure 2.2: A body with frame Ψi translated and rotated relative to a frame Ψ0 .

must be invertible, we have

 i −1   i T 
j  i −1 (Rj ) −(Rji )−1 pij (Rj ) −(Rji )T pij
Hi = H j = = (2.8)
0 1 0 1

which defines the matrix Hij as a simple combination of the elements of Hji .
Remark. The representation using homogeneous matrices is highly redundant;
the matrix contains sixteen numbers, whereas SE(3) is only six-dimensional.
Definition (2.1) constrains
 the structure
 of a homogeneous matrix: the bottom
row must be equal to 0 0 0 1 , and R must be an orthogonal matrix with de-
terminant 1. When homogeneous matrices are used in theoretical developments
and proofs (as they are in this thesis), these constraints are automatically satisfied
by the operations allowed on them. However, when they are used in numerical
computations and simulations, in which roundoff errors and other approxima-
tions occur, it should be checked whether the matrices remain homogeneous and
whether a numerical correction is necessary. 
Example 2.5. As an example of the described approach, consider Figure 2.2,
which shows a rigid ellipsoid with coordinate frame Ψi and a plane with frame
Ψ0 (the symbol Ψ0 is usually taken as the reference frame). To describe the rela-
tive configuration of the ellipsoid with respect to the plane, we can compute the
matrix Hi0 as
⎡ ⎤
cos(θ) 0 sin(θ) 0
⎢ 0 1 0 a⎥
Hi0 = ⎢⎣− sin(θ) 0 cos(θ) b ⎦
⎥ (2.9)
0 0 0 1
22 CHAPTER 2. MODELING OF RIGID MECHANISMS

Suppose we are interested in the point Q on the ellipsoid which has body-fixed
coordinates, say, xi = 0, y i = 2, z i = 1 (these are independent of the configuration
of the ellipsoid). Then to find the coordinates of the point Q relative to the plane,
i.e. expressed in Ψ0 , we compute
⎡ ⎤⎡ ⎤ ⎡ ⎤
cos(θ) 0 sin(θ) 0 0 sin(θ)
⎢ 0 1 0 a⎥ ⎢ ⎥ ⎢ ⎥
Q0 = Hi0 Qi = ⎢ ⎥ ⎢2⎥ ⎢ 2 + a ⎥ (2.10)
⎣− sin(θ) 0 cos(θ) b ⎦ ⎣1⎦ = ⎣cos(θ) + b⎦
0 0 0 1 1 1


2.1.2 Velocity of a rigid body

The next aspect in the kinematic analysis of a rigid body is the description of its
possible velocities. As in the case of configuration, there is a mixture of linear
velocities and angular velocities, and we seek to describe these two aspects as
one mathematical object.
Since we want to use a time-varying homogeneous matrix H(t) to describe
the position and orientation of a rigid body, it would be straightforward to use
d
a matrix Ḣ(t) := dt H(t) to describe its velocity. However, at all times the ma-
trix H(t) is constrained to be a homogeneous matrix, and so also the degrees of
freedom of Ḣ(t) are constrained, depending on the current value of H(t).
In order to find a better representation of velocity, we look more closely at the
constraints on Ḣ. From (2.1), we have
 
Ṙ(t) ṗ(t)
Ḣ(t) = with R(t)RT (t) = RT (t)R(t) = I (2.11)
0 0
If we compute the time-derivative of the constraint on R, we obtain
 T
0 = ṘT R + RT Ṙ = RT Ṙ + RT Ṙ (2.12)

which shows that the constraints on Ḣ can be translated into the constraint that
RT Ṙ must be a skew-symmetric matrix (plus the constraint that the bottom row
of Ḣ must be all zeros). This leads us to the notion of a twist and its use as a
representation for the velocity of a rigid body.
First, we define the tilde operator acting on elements of R3 as the bijective
linear mapping from R3 to the space of 3 × 3 skew-symmetric matrices such that
⎡ ⎤ ⎡ ⎤
0 −wz wy wx
w̃ := ⎣ wz 0 −wx ⎦ ∀ w = ⎣wy ⎦ ∈ R3 (2.13)
−wy wx 0 wz
 2.1. KINEMATICS OF RIGID BODIES 23

With this definition and following the notation of Stramigioli (2001), we can for-
mulate the definition1 of a twist.

Definition 2.6 (Twists). Let Hji (t) be a time-varying homogeneous matrix repre-
senting the relative configuration of a rigid body j relative to a body i. We define the
twist Tjk,i (twist expressed in coordinate frame Ψk ) of the rigid body as the vector
 
ωjk,i (t)
Tjk,i (t) := k,i ∈ R6 (2.14)
vj (t)

such that at all times t we have

 k,i   k i j i 
ω̃ vjk,i Ri (Ṙj Ri )Rk Rik ṗij − Rik Ṙji pjk
T̃jk,i := j = Hik Ḣji Hkj = (2.15)
0 0 0 0

where we omitted the explicit time-dependencies for clarity.

Since the tilde operator is a bijective mapping and since homogeneous matrices
are invertible, the space of allowed matrices Ḣji is bijectively parameterized by
the twists. The matrix Ḣji can be simply recovered from the twist as

Ḣji = Hki T̃jk,i Hjk (2.16)

The problem of constraints on the allowed velocities is thus avoided if velocities

are represented by twists instead of time-derivatives of a homogeneous matrices.
 As demonstrated in Appendix A.3, the space SE(3) is a Lie group, and homo-
geneous matrices can be used as matrix representation of the elements of SE(3).
The lie algebra se(3) of the group is defined as the tangent space to the identity
of the group. From Definition 2.6, we see that the twist is just the element of the
algebra se(3) corresponding to the translation of the tangent element Ḣ at H to
the identity. The right and left translation correspond to the choice Ψk = Ψi and
 Ψk = Ψj , respectively.
Apart from being a mathematically attractive representation of a constrained
higher-dimensional space, twists also have a clear geometric interpretation. To
see this, let us return to the use of a matrix Hi0 to compute the world coordinates
of a point q attached to a rigid body. We can compute the (linear) velocity Q̇0 of
the point q as the rigid body moves in space by using the twist:
 0,0 0 
0 d  0 i 0 i 0,0 0 i 0,0 0 ω̃i q + vi0,0
Q̇ = Hi Q = Ḣi Q = T̃i Hi Q = T̃i Q = (2.17)
dt 0
ˆ ˜T
1 Sometimes, such as in Murray et al. (1994), twists are defined as v T ω T , so with ω and v
ordered differently. This notation is not arbitrary, but depends on choosing either ray coordinates or
axis coordinates, as defined in screw theory, see e.g. Lipkin (1985).
24 CHAPTER 2. MODELING OF RIGID MECHANISMS

where the second equality follows from Q̇i = 0, since q is rigidly attached to the
body and hence its coordinates relative to Ψi are constant. The twist Ti0,0 is often
called the twist in world-coordinates, since it directly relates the positions Q0 and
velocities Q̇0 of points, i.e. when expressed in the world-fixed coordinate frame
Ψ0 . Similarly, the twist Tii,0 is called the twist in body-coordinates as it directly
relates positions and velocities of points expressed in the body-fixed frame Ψi .
The previous discussion shows how the twist can be interpreted as a combina-
tion of a linear velocity and an angular velocity: ωi0,0 defines a vector of rotation
through the origin of Ψ0 , and vi0,0 defines a vector of linear velocity, such that a
point q on the rigid body has a velocity relative to Ψ0 equal to q̇ 0 = ωi0,0 ∧q 0 +vi0,0 .
However, the choice of splitting between rotation and translation depends on
the choice of the reference frame Ψ0 ; if we move the reference frame to a differ-
ent location, both the rotation and translation part may change, even though the
velocity of the rigid body is still the same. An interpretation of a twist that is in-
dependent of the choice of reference frame is given by Charles’ Theorem, which
is based on the notion that every rigid motion can be decomposed uniquely in a
rotation around an axis plus a translation along the same axis.

Theorem 2.7 (Charles’ Theorem). Every twist T can be written as the sum of a
rotation around an axis and a translation along the same axis.
   
ω̂ 0
T =α +β (2.18)
r ∧ ω̂ ω̂

where ω̂ is the unit vector in the direction of the axis, r is any vector from the origin
of the reference frame to that axis, α is the magnitude of the rotation, and β is the
magnitude of the translation.

Proof. Consider a general twist T . We can distinguish three situations:

• ω = v = 0: choose α = β = 0 and ω̂ and r arbitrary (ω̂ and r have no
physical meaning here, since there is no motion).
• ω = 0, v = 0: choose α = 0, β =
v
, ω̂ = v v
and r arbitrary (r has no
physical meaning here, since a translational motion is fully defined by its
direction ω̂ and magnitude β).
• ω = 0: choose α =
ω
, ω̂ = ω
ω
, r = α1 ω̂ ∧ (v − ω̂ T v ω̂), and β = ω̂ T v.

With these choices of α, β, ω̂, and r, any twist can be expressed in the form of the
theorem. 
With a twist expressed in the form of Theorem 2.7, we can make a coordinate-
independent distinction between a purely translational motion (α = 0), a rota-
tional motion (β = 0), or a general so-called screw-motion (α = 0, β = 0). The
ratio of the numbers α and β is called the pitch.
2.1. KINEMATICS OF RIGID BODIES 25

Ψl

Ψk
Ψm

Ψj
Ψi

Figure 2.3: Three rigid bodies with several attached coordinate frames.

Finally, let us present some useful identities about twists and homogeneous
matrices that are used later in this thesis.
Lemma 2.8. Given the setup of Figure 2.3 with Ψi and Ψj rigidly attached to one
rigid body (the plane), Ψk and Ψl rigidly attached to another rigid body (the sphere),
and Ψm attached to the final rigid body (the cube). Then the following identities hold

(a) Tji,i = Tlk,k = 0

(b) T̃jm,k = Him T̃ji,k Hm
i
   
R 0 R p
(c) Tjm,k = AdHim Tji,k with AdH := for H =
p̃R R 0 1
(d) Tkj,j = −Tjj,k
j,j
(e) Tm = Tkj,j + Tm
j,l
   
d ω̃ 0 ω
(f) dt AdHik = AdHik adT i,k with adT := for T =
i ṽ ω̃ v

Proof. We prove each of the properties separately, using Definition 2.6 and the
rigidity of the objects.
(a) The twist Tji,i is defined in tilde form as T̃ji,i = Ḣji Hij . Since the frames
Ψi and Ψj are attached to the same body, we have Ḣji = 0, and hence also
Tji,i = 0. The same holds for Tlk,k .
(b) We can directly write

T̃jm,k = Hkm Ḣjk Hm

j
= Him Hki Ḣjk Hij Hm
i
= Him T̃ji,k Hm
i
(2.19)
26 CHAPTER 2. MODELING OF RIGID MECHANISMS

which gives a coordinate transformation rule for twists in tilde form.

(c) Expanding the expression obtained in (b), we find
 m   i,k  i 
R i pm ω̃j vji,k Rm −Rm
i m
pi
T̃jm,k = Him T̃ji,k Hm i
= i
0 1 0 0 0 1
 m i,k i m i,k i m m i,k

Ri ω̃j Rm −Ri ω̃j Rm pi + Ri vj
=
0 0
⎡ ⎤
     
i,k i,k m i,k
m
= ⎣ R i ωj − R i ω j p i + R i vj ⎦
m m

0 0
⎡ ⎤
  
i,k i,k i,k
m
= ⎣ R i ωj p̃m m m
i R i ω j + R i vj ⎦ (2.20)
0 0

where the third line follows since for all x ∈ R3 we have

 
 = (Rω) ∧ x = RRT ((Rω) ∧ x) = R (RT Rω) ∧ (RT x)
(Rω)x
 
= R ω ∧ (RT x) = Rω̃RT x (2.21)

 = Rω̃RT . Comparing the expression for T̃ m,k to the tilde

and hence (Rω) j
representation of AdHim Tji,k , we see that they are equal.
(d) From the time-derivative of the identity Hkj Hjk = I, we immediately obtain

0 = Ḣkj Hjk + Hkj Ḣjk = T̃kj,j + T̃jj,k (2.22)

(e) Looking at the tilde-form of the equation, we see

j,j j,j m d  j k l  m
T̃m = Ḣm Hj = H k H l H m Hj
 dt 
= Ḣkj Hm
k
+ Hkj Ḣlk Hm
l
+ Hlj Ḣm
l
Hjm
= Ḣkj Hjk + Hkj Ḣlk Hjl + Hlj Ḣm
l
Hlm Hjl
= T̃kj,j + 0 + T̃m
j,l
(2.23)
where the zero follows since Ψk and Ψl are attached to the same body.
(f) By definition of AdH , we have
   
d Ṙi 0 Rik ω̃ii,k 0
AdHki = ˙k k k k k = (2.24)
dt p̃i Ri + p̃i Ṙi Ṙik Rik ṽii,k + p̃ki Rik ω̃ii,k Rik ω̃ii,k
which can be written in the proposed form.

2.2. KINEMATICS OF RIGID MECHANISMS 27

2.2 Kinematics of rigid mechanisms

We define a rigid mechanism as a (finite) number of rigid bodies, interconnected
by ideal joints. Rigid bodies have been discussed in the previous section, and now
we proceed to discuss the mathematical formulation of ideal joints using the same
concepts of homogeneous matrices and twists. We then combine rigid bodies and
ideal joints to obtain a kinematic description of general rigid mechanisms.
As described before, an ideal joint (sometimes also called a kinematic pair) is a
purely kinematic relationship between the motion of two rigid bodies; some rela-
tive velocities are allowed and some are not, independently of the forces applied
to the bodies. The following definition characterizes the class of joints considered
in this thesis.

Definition 2.9 (Globally Parameterized Rigid Joint). A globally parameterized

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

• a matrix Q ∈ Q̄, parameterizing the relative configuration as Hji = Hji (Q)

• a vector v ∈ Rk , parameterizing the relative twist as Tji,i = X(Q)v

where X(Q) depends smoothly on Q and v = VQ (Q̇) with VQ invertible and linear
in Q̇. Furthermore, there exists a mapping FQ : Rk → Q̄ satisfying

• FQ (φ) assigns local coordinates φ ∈ Rk to a neighborhood of every Q ∈ Q̄.

• FQ (φ) is twice continuously differentiable in φ
• FQ (0) = Q

The function FQ (φ) defines a local coordinate patch with coordinates φ around
every allowed configuration parameterized by Q. If the configurations around Q
are described by coordinates φ, the velocity v can also be expressed as
 
∂FQ (φ)
v = VQ (Q̇) = VFQ (φ) φ̇ (2.25)
∂φ

i.e. in terms of the time derivatives of the local coordinates. This property is used
in the derivation of the dynamic equations in Theorem 2.17.
Note that the linearity of the subspace implies that no end-stops or speed-
bounds are considered. Furthermore, since the dimension of the vector φ is equal
to the dimension of v, we consider only joints for which the space of allowed
relative configurations (described by φ) has the same dimension as the space of
instantaneously allowed velocities (described by v). This type of joint is called a
holonomic joint. Section 2.4 discusses an extension of the results to nonholonomic
 28 CHAPTER 2. MODELING OF RIGID MECHANISMS

joints, i.e. joints for which the space of allowed instantaneous velocities has a
smaller dimension than the space of allowed configurations.
The technical conditions on the mapping F are trivially satisfied if Q can be
chosen as a k-vector (in which case we can take FQ (φ) = Q + φ). Thus, the
often encountered rotational and prismatic joints are part of the class of globally
parameterized rigid joints, as is shown in Example 2.10 below.
 Another case for which the conditions on local coordinates are satisfied is
when the space of allowed configurations of the joint is a Lie group, with Q the
matrix-representation of the Lie group and v the vector representation of the cor-
responding Lie algebra. The global mapping F can then be chosen as the map-
ping of exponential coordinates φ around Q. More details about exponential co-
 ordinates can be found in Appendix A.3.
Example 2.10. As examples, we show how the commonly encountered joints of
Figure 2.4 can be described in the form required by the definition above, i.e. in
terms of parameterized twists and homogeneous matrices. For each joint, we do
not consider possible mechanical end-stops, even though the figures may suggest
their existence.
(a) The rotational joint is frequently used in robotics modeling, as robots often
contain several rotational joints. For the joint in the figure, the frames Ψ1
and Ψ2 differ by a consecutive fixed translation a along y, variable rotation
of q about x, and fixed translation b along y, which can be written as
⎡ ⎤
1 0 0 0
⎢0 cos(q) − sin(q) a + b cos(q)⎥
H21 (q) = ⎢
⎣0 sin(q) cos(q)
⎥ (2.26)
b sin(q) ⎦
0 0 0 1
in which we chose simply Q = q to parameterize the allowed configura-
tions. The relative twist is a pure rotation around x displaced over a dis-
tance a along y, or in vector form
 T
T21,1 = X(q)v = 1 0 0 0 0 −a q̇ (2.27)
which shows that we choose v = q̇. Since the configuration parameter q is
just an unconstrained scalar, the local coordinate mapping can be chosen
Fq (φ) = q + φ.
(b) The prismatic joint is a simple 1 DoF joint that allows only translation along
one axis, in this case x. If we denote by q = 0 the situation that Ψ1 and Ψ2
are coincident, then the relative configuration can be written as
⎡ ⎤
1 0 0 q
⎢0 1 0 0⎥
H21 (q) = ⎢
⎣0 0 1 0⎦
⎥ (2.28)
0 0 0 1
2.2. KINEMATICS OF RIGID MECHANISMS 29

z
x z z
Ψ1
z q
Ψ1
x Ψ2 y
x Ψ2
q y

(a) Rotational joint (one DoF). (b) Prismatic joint (one DoF).

Ψ3 y
x
Ψ2 z z Ψ1
y x
y
z q3 q
y
x
x x Ψ2 y
1
q
Ψ1 q2

(c) Planar motion (three DoF). (d) Two-gear system (one DoF).

z
z Ψ2 z
x y Ψ1 y
x
x
x Ψ2 y
Ψ1 y
(e) Free motion (six DoF). (f) Ball joint (three DoF).

Figure 2.4: Examples of globally parameterized rigid joints.

30 CHAPTER 2. MODELING OF RIGID MECHANISMS

The corresponding relative twist is a simple linear velocity, or

 T
T21,1 = X(q)v = 0 0 0 1 0 0 q̇ (2.29)

So also for this joint, we can choose Q = q and v = q̇, and hence again
Fq (φ) = q + φ.
(c) A planar joint is used to describe free translation in a plane plus free ro-
tation around the axis perpendicular to that plane. We can choose three
coordinates as in the figure to describe the relative configuration as
⎡ ⎤
cos(q 3 ) − sin(q 3 ) 0 q1
⎢ sin(q 3 ) cos(q 3 ) 0 q2 ⎥
H12 (Q) = ⎢
⎣ 0
⎥ (2.30)
0 1 0⎦
0 0 0 1
 T
such that in this case Q = q 1 q2 q3 . The corresponding relative twist
can be described as
⎡ ⎤T ⎡ 1⎤
0 0 0 1 0 0 q̇
T21,1 = X(Q)v ⎣0 0 0 0 1 0⎦ ⎣q̇ 2 ⎦ (2.31)
0 0 1 q2 −q 1 0 q̇ 3

which shows again a choice v = q̇. Thus, also for planar joints, we can use
the coordinate mapping FQ (φ) = Q + φ with φ ∈ R3 .
(d) The two-gear system is an example of a system with a kinematic loop: the
two gearwheels are connected by rotational joints to a common frame, but
also to each other by the constraint that the linear velocity of the contact
point is equal on both gearwheels (ideal gear systems have no flexibility
or backlash). If we assume for conciseness that the gearwheels have unit
radius, we obtain as the constraint of the contact point p that

T̃21,1 P 1 = T̃31,1 P 1 (2.32)

⎡ 2
⎤⎡ ⎤ ⎡ ⎤⎡ ⎤
0 0 −q̇ 0 0 0 −q̇ 3 0 0 0
⎢0 ⎥ ⎢ ⎥
0⎥ ⎢−1⎥ ⎢q̇ 3 ⎢ 0⎥ ⎢ ⎥
⎢ 2 0 0
=
0 0 ⎥ ⎢−1⎥ (2.33)
⎣q̇ 0 0 0⎦ ⎣ 1 ⎦ ⎣ 0 0 0 0⎦ ⎣ 1 ⎦
0 0 0 0 1 0 0 0 0 1

where we expressed the velocity of the contact point using the twists as in
(2.17). From this equation, we find q̇ 3 = −q̇ 2 , i.e. that the angular speed of
Ψ3 must be equal but opposite to that of Ψ2 . Taking as parameter q = q 2 =
−q 3 and as initial condition the configuration shown in the figure, we can
2.2. KINEMATICS OF RIGID MECHANISMS 31

describe the relative configurations as

⎡ ⎤
0 cos(q) − sin(q) 0
⎢−1 0 0 −1⎥
H21 (q) = ⎢
⎣ 0 sin(q) cos(q)
⎥ (2.34)
0⎦
0 0 0 1
⎡ ⎤
cos(q) sin(q) 0 0
⎢− sin(q) cos(q) 0 0⎥
H3 (q) = ⎢
1
⎣ 0
⎥ (2.35)
0 1 1⎦
0 0 0 1

and the relative twists as

 T
T21,1 = 0 −1 0 0 0 0 q̇ (2.36)
 T
T31,1 = 0 0 −1 0 0 0 q̇ (2.37)

hence just two rotational joints as in (a), but now both depending on the
same states q and q̇. In a similar way, this approach can be used for systems
of more than two gears.
(e) Free (6 DoF) relative motion of two rigid bodies can be seen as a degen-
erate joint (since in fact nothing is joined), but can still be described as a
globally parameterized rigid joint. We can just take Q = H21 and v = T21,1 ,
and hence X(Q) = I, and VQ (Q̇) as the vector representation of Q̇Q−1 . As
global mapping FQ (φ), we can use the exponential coordinates described
in Appendix A.3.
(f) The spherical joint allows two bodies to have a fixed relative displacement,
but arbitrary rotation. If we choose a rotation matrix R to describe the free
relative rotations of the two concentric spheres defining the joint, and if we
assume for conciseness that Ψ1 and Ψ2 are displaced by vectors p1 and p2
from the centers of rotation, respectively, then their relative configuration
can be described by
     
1 I p1 R 0 I p2 R Rp2 + p1
H2 (R) = = (2.38)
0 1 0 1 0 1 0 1

while the relative twist can be written as

   
1,1 ṘRT −ṘRT p1 I
1 2
T̃2 = Ḣ2 H1 = or T21,1 = ω (2.39)
0 0 p̃1

where ω̃ := ṘRT . For this joint, we choose Q = R, v = ω, VQ (Q̇) the vector

representation of ṘRT , and for the global mapping FQ (φ) we choose again
exponential coordinates.
32 CHAPTER 2. MODELING OF RIGID MECHANISMS

3 E32
E63
B2 B3
2 5
6 E21 E53 B6
0
1 4 E10 B5
E41 B4
B0 B1
7
E71
9
8 8 B9
B7 E87 B E9
8

Figure 2.5: Example of a system and its kinematic graph. The graph can be made
loop-free, e.g. by removing (in first instance) the joint between B4 and B5 .

The examples show how general kinematic relations between rigid bodies can be
represented as globally parameterized rigid joints. 
We can now describe the kinematics of a rigid mechanism as an interconnec-
tion of ideal joints attached to rigid bodies. Provided with a practical mechanism
(or its preliminary design), it is up to the modeler to decide what parts of the
mechanism will be considered rigid and what joints will be considered ideal.
The topology of the idealized mechanism can be represented by a so-called
kinematic graph (more on graphs in Appendix B.2), in which each vertex repre-
sents a rigid body, and each edge represents an ideal joint; Figure 2.5 shows an
example. If the graph contains loops (cyclic paths), this means that the mech-
anism contains kinematic loops. Unfortunately, the kinematics and dynamics
descriptions of this section and the following are only suitable for mechanisms
without kinematic loops. Therefore, the graph should be made loop-free, either
(if possible) by formulating the mechanism in a different loop-free way, such as
was done for the two-gear system in the example, or by initially removing a joint
to break the loop, and modeling it in a later phase in terms of a constraint force
(see Section 2.4).
For a loop-free kinematic graph, we use the following labeling conventions.
First, the vertex labeled B0 represents the reference frame. Second, a vertex Bi
represents a rigid body with coordinate frame Ψi . Third, an edge Eji denotes an
ideal joint that connects body j to body i, where Bi is an element of the path from
Bj to B0 (i.e. in the graph, Bi is closer to B0 than Bj ).
In order to obtain a mathematical description of the kinematics of the mech-
anism, we use the techniques described before to parameterize every matrix Hji
corresponding to an edge Eji using joint coordinates Qj . Furthermore, we write
the relative twist as Tji,i = Xji (Qj )v j with v j the coordinates for the velocities.
We combine the local descriptions of the individual joints to obtain an expression
for the configuration and twist of each rigid body with respect to the reference
2.2. KINEMATICS OF RIGID MECHANISMS 33

frame. This is presented in the following lemma, which is a generalization of the

geometric Jacobians as defined for example by Stramigioli (2001) and by Murray
et al. (1994) — in the latter reference, it is called the spatial manipulator Jacobian.

Lemma 2.11 (Geometric Jacobian). Given a loop-free rigid mechanism comprised

of rigid bodies Bi with coordinate frames Ψi , and n globally parameterized rigid
joints Eji with associated relative twists Tji,i = Xji (Qj )v j . The twist of a body Bi
relative to the reference frame Ψ0 can then be written as
⎡ 1⎤
v
0,0  1 1
⎢ . ⎥
Ti = Ji (Q)v := σi AdHα0 X1 (Q ) · · · σi AdHω0 Xn (Q ) ⎣ .. ⎦ (2.40)
α n ω n

vn

where Ji is called the (geometric) Jacobian of body i, and where σij is defined as

j 1 if joint Ej is in the path from Bi to B0 , for some ∈ {1, . . . , n}
σi =
0 otherwise

Proof. The proof follows directly from Lemma 2.8, parts (c) and (e), since for a
general body in the mechanism, we can write its twist as the sum of all the twists
of the consecutive joints between that body and the reference frame, which can
be expressed mathematically in the form of the lemma. The variables σij define
the topological structure of the mechanism: only the twists of the joints in the
path from Bi to B0 influence the twist of Bi , not the joints further away from the
reference body or joints in different branches of the mechanism. 
Example 2.12. As an example of the proposed techniques, consider the (pla-
nar) mechanism of Figure 2.6, containing a horizontal prismatic joint, a rotational
joint, a gear system, and another prismatic joint. With the labels and coordinate
frames as indicated in the figure, the kinematic graph can be drawn as shown
(where the two gear wheels should be considered one joint). Assuming unit-
radius gearwheels, the relative configurations can be described as
⎡ ⎤ ⎡ ⎤
1 0 0 0 1 0 0 0
⎢0 1 0 −q 1 ⎥ ⎢0 cos(q 2 ) − sin(q 2 ) 0⎥
H10 = ⎢⎣0 0 1
⎥
⎦ H21 = ⎢⎣0 sin(q 2 ) cos(q 2 ) 1⎦
⎥
0
0 0 0 1 0 0 0 1
⎡ ⎤ ⎡ ⎤ (2.41)
1 0 0 0 1 0 0 0
⎢0 cos(q 2 ) sin(q 2 ) 0⎥ ⎢0 1 0 0 ⎥
H31 = ⎢ ⎥ 3 ⎢
⎣0 − sin(q 2 ) cos(q 2 ) 3⎦ H4 = ⎣0 0 1 q 3 ⎦
⎥

0 0 0 1 0 0 0 1
34 CHAPTER 2. MODELING OF RIGID MECHANISMS

Ψ4
B4

z
3
q
E43

y
B3
Ψ3
B2
z

(gears)
y

E21
z Ψ2 y
q2
B1
Ψ1 z z
Ψ0 E10
y y
q1 B0

Figure 2.6: Example of a rigid mechanism and its kinematic graph.

and the relative twists can be described as

 T
T10,0 = 0 0 0 0 −1 0 q̇ 1
 T
T21,1 = 1 0 0 0 1 0 q̇ 2
 T (2.42)
T31,1 = −1 0 0 0 −3 0 q̇ 2
 T
T43,3 = 0 0 0 0 0 1 q̇ 3

which results in the following Jacobian matrix for the end effector Ψ4 :
⎡⎛ ⎞ ⎛ ⎞ ⎛ ⎞ ⎛ ⎞⎤ ⎡ ⎤
0 1 −1 0 0 −1 0
⎢⎜0⎟ ⎜0⎟ ⎜ 0 ⎟ ⎜ 0 ⎟⎥ ⎢ 0 0 0 ⎥
⎢⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟⎥ ⎢ ⎥
⎢⎜0⎟ ⎜0⎟ ⎜ 0 ⎟ ⎜ 0 ⎟⎥ ⎢ 0 0 0 ⎥
⎢ ⎜ ⎟ ⎜
J4 = ⎢1⎜ ⎟ 0⎜ ⎟+1⎜ ⎟ 1⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎥ ⎢ ⎥ (2.43)
⎟⎥ = ⎢ 0 ⎥
⎢⎜0⎟ ⎜0⎟ ⎜ 0 ⎟ ⎜ 0 2 ⎟⎥ ⎢ 0 0 ⎥
⎣ ⎝−1⎠ ⎝ 1 ⎠ ⎝−3⎠ ⎝ sin(q ) ⎠⎦ ⎣−1 −3 sin(q 2 ) ⎦
0 q1 q1 cos(q 2 ) 0 q 1 cos(q 2 )
 T
such that T40,0 = J4 v, with v = q̇ 1 q̇ 2 q̇ 3 . The Jacobians of the other links
can be constructed in a similar way. 
2.3. DYNAMICS OF OPEN RIGID MECHANISMS 35

2.3 Dynamics of open rigid mechanisms

Now that we have a suitable mathematical description language for the kine-
matics of a rigid mechanism, we can proceed to describing its dynamics, i.e. the
relation between the forces acting on a mechanism and its motion.

2.3.1 Forces on rigid mechanisms

We first look at the effect of forces on rigid bodies. We want to describe these
forces in such a way, that the power P associated with the action of forces on
a rigid body with a twist T can be formulated as P = W T T , just like we have
P = f T v for point masses with velocity v and force f . Thus, we look for an
expression W describing somehow the forces as a vector which, when paired
with the twist, gives the power.
Consider a rigid body with attached coordinate frame Ψi and relative twist
Tii,k relative to some body k, and a single arbitrary force f acting on it. Let fi
denote the 3 × 1 column vector expressing the force magnitude in Ψi , and let ri
denote the 3 × 1 column vector with the coordinates of the point on the body at
which the force acts, expressed in Ψi . Then we can compute the power associated
with the force as the (dual) product of the force fi and the velocity v i (expressed
in Ψi ) of the point at which the force acts, or
 i  
  r   ri
P = fiT v i = fiT 0 T̃ii,k = fiT ω̃ii,k vii,k
1 1
 i,k   i T
  ωi  i  r̃ fi
= fiT −r̃i I i,k = fi −r̃
T
I Tii,k = Tii,k (2.44)
vi fi
The final step shows how we obtain the desired expression of power as P =
W T T by taking W to be the wrench in the following definition. It is also the only
possible solution for W , as the equation for power should hold for all possible
twists.
Definition 2.13 (Wrenches). Let f j for j = 1 . . . k be a system of k forces acting
on a rigid body i with attached coordinate frame Ψi . Let fij be the coordinates in Ψi
of the force f j , and let rji be the coordinates in Ψi of the point of attachment of the
force on the body. The wrench W i,i associated with this system of forces is defined as
the 6 × 1 column vector
k  i j
 k  i 
i,i r̃ f
j i rj ∧ fij
W = = (2.45)
j=1
fij j=1
fij

where the first superscript denotes the coordinate frame in which the wrench is ex-
pressed, and the second superscript denotes the body on which the wrench is applied.
 36 CHAPTER 2. MODELING OF RIGID MECHANISMS

The expression for power (2.44) is linear in the forces, and this allows to take
linear combinations of wrenches and obtain again a valid wrench.
 A special ex-
ample of this is when the term f j is zero, while the term r̃j f j is nonzero.
This corresponds to a system of forces where the total force is zero while the total
moment is nonzero, i.e. to the case of a pure torque on a body.
To obtain a method for changing the coordinates of a wrench (i.e. expressing
the same system of forces on the same body in a different coordinate frame Ψj ),
we again look at the expression for power. Since power is a physical quantity
independent of the coordinates, we should have

P = (W j,i )T Tij,k = (W i,i )T Tii,k

   T
= (W i,i )T AdHji Tij,k = AdTH i W i,i Tii,k (2.46)
j

and since this should hold for all twists Tii,k , we find the following coordinate
transformation rule for wrenches.

W j,i = AdTH i W i,i (2.47)

j

 Wrenches are linear operators on twists, as can be seen by the construction

of a wrench using the expression for power. In Lie-group terms (Appendix A.3),
since twists are elements of the lie algebra se(3), wrenches are elements of the
dual space of se(3), denoted se∗ (3). When an element of the dual space (a wrench)
is applied to an element of the space itself (a twist), the result is a coordinate-
 independent real number (the power).
When considering rigid mechanisms, we have seen how the twist of a rigid
link in the mechanism can be expressed more efficiently in terms of coordinates v
together with the Jacobian (Lemma 2.11). We can also represent wrenches on rigid
mechanisms efficiently by using a vector τ (commonly called the joint torques)
which is collocated with v, i.e. such that τ T v equals the power associated with
the forces τ acting on the mechanism with velocity v. We can relate the external
wrenches to the joint torques as in the following lemma.

Lemma 2.14. For a mechanism with kinematics described by Lemma 2.11, an exter-
nal wrench W 0,i acting on body i has the same effect on the mechanism as a collection
of joint torques τ equal to τ = JiT W 0,i , where Ji is the Jacobian of body i and τ is
collocated with v.

Proof. The power associated with the wrench should be independent of whether
it is expressed in joint variables (v and τ ) or work space variables (T and W ), and
hence the following equality holds.

τ T v = (W 0,i )T Ti0,0 = (W 0,i )T Ji v = (JiT W 0,i )T v (2.48)

2.3. DYNAMICS OF OPEN RIGID MECHANISMS 37

z z
Ψi y q Ψi y q
z z
Ψj r Ψj
r
−f f
f y y

(a) common practical implementation (b) implementation for this example

Figure 2.7: A linear actuator attached to a rotational joint.

Since this should hold for all v, we have the expression for τ in the theorem. 
Note that the mapping of Lemma 2.14 is in general not injective, since the
Jacobian can have a kernel, i.e. certain wrenches are projected to zero and have
no effect on the mechanism. Physically, the wrenches in the kernel of JiT are
precisely those cancelled out by the constraint forces in the joints.
As an example, consider the effect of gravity on a rigid body of mass m. If we
assume gravity to act in the negative z direction of Ψ0 and have magnitude g, the
wrench Wgi,i associated with the gravitational force on body i equals
⎡ ⎤
    0
r̃g fg r̃g (R0i )ez
Wgi,i = = −mg with ez = ⎣0⎦ (2.49)
fg R0i ez
1

where R0i is the rotation part of H0i and rg is the location of the center of mass
of body i expressed in Ψi (often equal to zero). The equivalent joint torque τg is
then given by τg = JiT AdTH i Wgi,i . This expression can be used in the dynamics
0
equation for a rigid mechanism, discussed later on in this thesis.
As another example, consider Figure 2.7, showing a rotational joint (with an-
gle q) and linear actuator (force f ) attached to it. We can compute the equiv-
alent torques by computing the effect of the wrenches on both bodies i and j.
The practical implementation of the actuator would be as in Figure 2.7(a), but to
avoid having to deal with the resulting kinematic loop, we implement it as in Fig-
ure 2.7(b): we neglect the mass of the actuator and consider it a pure force source,
with equal forces f acting in opposing directions on the two connected links.
In this case, the mechanism does not have kinematic loops, and since the two
connected links are part of the same branch in the kinematic tree (they are still
connected by the rotational joint), their Jacobians are almost equal, except for one
extra nonzero column in one of the Jacobians, corresponding to the link that is
furthest away from the reference body. Without loss of generality, we assume
that Jj has an extra column, which by definition equals AdHi0 Xji,i . The Jacobian
 38 CHAPTER 2. MODELING OF RIGID MECHANISMS

Jj can then be expressed as

 
Jj = Ji + 0 ··· 0 AdHi0 Xji,i 0 ··· 0 (2.50)

Using this expression, we can compute the torques equivalent to the actuator
forces as follows.

τ = JiT W 0,i + JjT W 0,j

    T   
−r̃f r̃f
T
= Ji AdH i T
+ JiT + 0 · · · 0 AdHi0 Xji,i 0···0 AdTH i
0 −f 0 f
 T  
i,i r̃f
= 0 · · · 0 AdHi0 Xj 0 · · · 0 AdH i
T
0 f
 T r̃f 
= 0 · · · 0 Xji,i 0 · · · 0 (2.51)
f

This shows that for internal wrenches acting on two bodies connected by a single
joint q i , the equivalent torque is zero except for τi . Furthermore, the torque τi is
equal to the inner product of the wrench and the matrix describing the relative
motion allowed by the joint.

2.3.2 Kinetic co-energy of rigid mechanisms

Let us now proceed to define the kinetic co-energy of a rigid body. Again, we
want to express this energy in terms of the twist of the body instead of the ve-
locities of all its comprising points. Since we later want to apply Newton’s law
to describe the dynamics, we should express the kinetic co-energy in terms of
velocities relative to an inertial frame, i.e. a coordinate frame (denoted Ψ∅ ) that
is moving at a constant velocity relative to the distant stars. In this thesis, we
make the common approximation that Ψ∅ is rigidly attached to the earth and we
neglect the absolute motion of the earth.
 In the formal definition described in Appendix B.1, energy is a function of the
state of a dynamic element, not a function of one of the port variables. However,
it is often more convenient to describe the storage in terms of a port variable, in
which case the storage function is properly called co-energy to distinguish it from
the real energy. As the kinetic storage function above is expressed as a function
 of the twist, i.e. a port variable, it is called the kinetic co-energy.
Given an inertial frame Ψ∅ and a rigid body i with attached frame Ψi , we can
write the kinetic co-energy Ukp of a point p on the body with body-coordinates P i ,
2.3. DYNAMICS OF OPEN RIGID MECHANISMS 39

velocity vp∅ expressed in Ψ∅ , and mass mp as

  i T  i
1 1 p p
Ukp = mp (vp∅ )T vp∅ = mp Hi∅ T̃ii,∅ Hi∅ T̃ii,∅
2 2 1 1
      
1  i T  −ω̃i i,∅ ∅
0 (Ri )T 0 Ri∅ p∅i ω̃ii,∅ vii,∅ pi
= mp (p ) 1
2 (vii,∅ )T 0 (p∅i )T 1 0 1 0 0 1
  i   i  
1 i,∅ T i,∅ T p̃ 0 (Ri∅ )T Ri∅ (Ri∅ )T p∅i −p̃ I ωii,∅
= mp (ωi ) (vi )
2 I 0 (p∅i )T Ri∅ 1 + (p∅i )T p∅i 0 0 vii,∅
 i T i 
1 (p̃ ) p̃ p̃i i,∅
= mp (Tii,∅ )T Ti (2.52)
2 (p̃i )T I

which shows that the kinetic co-energy of a point mass can be expressed as a
quadratic function of the twist in body coordinates that is independent of the
relative configuration Hi∅ of the body. If we sum (by means of an integral) the
kinetic co-energy of all the point masses of the body, we obtain an expression for
the total kinetic co-energy of a rigid body equal to

1 i,∅ T i,i i,∅

Uk = (T ) I Ti (2.53)
2 i
with I i,i called the inertia matrix of the rigid body, defined as
 T
i,i (p̃i (x))T p̃i (x) p̃i (x)
I := m(x) dx (2.54)
B (p̃i (x))T I

and m(x) and pi (x) the mass density and body-fixed coordinates of the point x,
respectively, and where the integral is taken over the volume B of the rigid body.
By construction, the inertia matrix is positive semi-definite. Furthermore, if a
rigid body has a finite volume and mass distribution, then the inertia matrix will
be strictly positive-definite, meaning that for every non-zero twist the body has
nonzero kinetic co-energy.
If the coordinate frame on the body is chosen to be in the center of mass of
the body with the axes of the frame aligned with the principle directions of the
inertia ellipsoid of the rigid body, then the inertia matrix is a diagonal matrix of
the form
 
I i,i = diag Jx Jy Jz M M M (2.55)

with M the total mass of the body, and Jx , Jy , and Jz the moments of inertia
around the axes x, y, and z of Ψi . The parameters in this matrix are often easy to
measure or compute for a given rigid body, and therefore the body-fixed frames
are often chosen in this way.
40 CHAPTER 2. MODELING OF RIGID MECHANISMS

If the inertia matrix is known in one coordinate frame, it can also be expressed
in a different coordinate frame by using a suitable transformation rule. Since the
kinetic co-energy of a body does not depend on the chosen coordinate frame, we
have
1 j,∅ T j,i j,∅ 1 1
Uk = (T ) I Ti = (Tii,∅ )T I i,i Tii,∅ = (Tii,∅ )T AdTH i I i,i AdHji Tij,∅ (2.56)
2 i 2 2 j

and since this should hold for all twists, we obtain the following transformation
rule for inertia matrices.

I j,i = AdTH i I i,i AdHji (2.57)

j

With the kinetic co-energy of one rigid body known, we can express the kinetic
co-energy of a rigid mechanism as the sum of the kinetic co-energies of all the
rigid bodies in the mechanism. This is expressed formally in the following lemma.

Lemma 2.15 (Kinetic Co-Energy of a General Rigid Mechanism). Given a

rigid mechanism consisting of m rigid links and n globally parameterized rigid joints
as defined in Definition 2.9, and with kinematics described by a loop-free kinematic
graph and corresponding Jacobians as in Lemma 2.11. The kinetic co-energy of the
mechanism is equal to

1 T
Uk = v M (Q)v (2.58)
2
 T
where v = v 1 · · · v n with v i the velocity state of link i, and where the mass
matrix M (Q) is defined as


m
M (Q) := JiT (Q) AdTH i (Q) I i,i AdH∅i (Q) Ji (Q) (2.59)
∅
i=1

with Ji (Q) the Jacobian of link i and I i,i the body-fixed inertia matrix of link i.

Proof. The kinetic co-energy of a mechanism of several rigid bodies is equal to

the sum of the kinetic co-energies of the bodies. The kinetic co-energy of body i
can be written as (2.53), which, using the Jacobian, can be expressed as

1 1  
(Uk )i = (AdH i Ti∅,∅ )T I i,i (AdH i Ti∅,∅ ) = v T JiT (Q) AdTH i I i,i AdH i Ji (Q) v
2 ∅ ∅ 2 ∅ ∅

Summing this expression for all rigid bodies gives the total kinetic co-energy of
the mechanism. 
2.3. DYNAMICS OF OPEN RIGID MECHANISMS 41

2.3.3 Dynamic equations of rigid mechanisms

For the derivation of the dynamics of a rigid mechanism, we start from the Euler-
Lagrange equations (Goldstein 1980). For a mechanical system with generalized
coordinates q and q̇, kinetic co-energy Uk (q, q̇) = 12 q̇ T M (q)q̇ with M invertible,
potential energy Up (q), and input forces u collocated with y = B T (q)q̇, the time-
evolution of q is described by the differential equation
 
d ∂L ∂L
− = B(q)u (2.60)
dt ∂ q̇ ∂q

with the Lagrangian L(q, q̇) = Uk (q, q̇) − Up (q). If we define p := ∂L

∂ q̇ = M (q)q̇,
this can be written equivalently in port-Hamiltonian form as
     ∂H   
d q 0 I ∂q 0
= + u
dt p −I 0 ∂H
∂p
B(q)
  (2.61)
  T
∂H
∂q
y = 0 B (q) ∂H
∂p

with the Hamiltonian equal to H(q, p) = pT q̇ − L(q, q̇) = 12 pT M −1 (q)p + Up (q),

and (u, y) defining a power port.
These equations can be generalized to what are known as the Boltzmann-
Hamel equations, as described for example by Whittaker (1998). Instead of us-
ing q̇ to describe the velocity, we use a general vector v of the form v = S(q)q̇
with S(q) a continuously-differentiable invertible matrix. Using these coordi-
nates (sometimes called quasi-coordinates), the dynamics can be written as fol-
lows.

Lemma 2.16 (Boltzmann-Hamel Equations). Given a mechanical system with

generalized coordinates q, velocities v = S(q)q̇ (with S invertible and continuously
differentiable), inputs u collocated with v, potential energy Up (q) and kinetic co-
energy expressed as Uk (q, v) = 12 v T M (q)v. If we define p := M (q)v, we can write
the dynamics as
   0 −1
   
∂H
d q  T TS  ∂q 0
= T
∂H + I u (2.62)
dt p −S −T S −T ∂ (S ∂q
p)
− ∂(S p)
∂q S −1
∂p
 
  ∂H
∂q
v = 0 I ∂H
∂p

where the energy equals H(q, p) = 12 pT M −1 (q)p + Up (q).

42 CHAPTER 2. MODELING OF RIGID MECHANISMS

Proof. The kinetic co-energy Uk can be written as a function of q̇ instead of v

simply by substituting the relation between v and q̇:

1 T 1
Uk = v M (q)v = q̇ T S T (q)M (q)S(q)q̇ (2.63)
2 2
Using this expression, we can write the Lagrangian as a function of q and q̇ as

1 T T
L(q, q̇) = q̇ S (q)M (q)S(q)q̇ − Up (q) (2.64)
2
and its partial derivatives with respect to q and q̇ can be computed as
 
∂L 1 ∂(q̇ T S T (q)p) ∂(v T M (q)v) ∂(pT S(q)q̇) ∂Up (q)
= + + − (2.65)
∂q 2 ∂q ∂q ∂q ∂q
∂L
= S T (q)M (q)S(q)q̇ = S T (q)p (2.66)
∂ q̇

where we used the definition of p in the theorem, and where we explicitly de-
noted the variables depending on q. Substituting these expressions in the Euler-
Lagrange equations (2.60) (with B = S T since u is collocated with v), we obtain
 
d  T  1 ∂(q̇ T S T (q)p) ∂(v T M (q)v) ∂(pT S(q)q̇) ∂Up (q)
ST u = S p − + + +
dt 2 ∂q ∂q ∂q ∂q
∂(S T (q)p) ∂ T (S T (q)p) 1 ∂(v T M (q)v) ∂Up
= q̇ + S T ṗ − q̇ − +
∂q ∂q 2 ∂q ∂q
−1
∂(S T (q)p) ∂ T (S T (q)p) T
1 ∂(p M (q)p) ∂Up
= q̇ + S T ṗ − q̇ + +
∂q ∂q 2 ∂q ∂q
∂(S T (q)p) ∂ T (S T (q)p) ∂H
= q̇ + S T ṗ − q̇ + (2.67)
∂q ∂q ∂q

where the third equality follows from the identity

∂I ∂(X −1 X) ∂X −1 ∂X
0= = = X + X −1 (2.68)
∂a ∂a ∂a ∂a
which holds for any invertible matrix X and any variable a. The resulting dy-
namic equation can be written directly in the form of (2.62). 
The Boltzmann-Hamel equations serve as a basis for the derivation of the dy-
namic equations for general rigid mechanisms as presented in the following the-
orem. This theorem gives the dynamic equations for general rigid mechanisms
without kinematic loops, comprised of rigid links and interconnected by globally
parameterized rigid joints.
2.3. DYNAMICS OF OPEN RIGID MECHANISMS 43

Theorem 2.17 (Dynamics of a General Rigid Mechanism). Given a rigid

mechanism as in Lemma 2.15 with total energy equal to kinetic plus potential energy,
and with power-port (u, v). Its dynamics are described by the following equations.

Q̇ = VQ−1 (v) (2.69)

 !
∂H ∂ (S p) ∂(S p) !!
T T T
ṗ = −S0−T + S0−T − ! S0−1 M −1 p + u (2.70)
∂φ ∂φ ∂φ φ=0

v = M −1 (Q)p (2.71)

with the energy function H(Q, p) = 12 pT M −1 (Q)p + Up (Q) and where we defined
 
∂
i i i i
Si (Q , φ )φ̇ := VFi (φi ) i i
Fi (φ )φ̇ ∀ φ̇i (2.72)
∂φi
 
S(Q, φ) := diag S1 (Q1 , φ1 ) · · · Sn (Qn , φn ) (2.73)
S0 := S(Q, 0) (2.74)
!
∂H ∂H ∂Fi !!
:= ◦ ∀ φ̇i (2.75)
∂φi ∂Qi ∂φi !φ=0

with Fi (φi ) the local coordinate function of the globally parameterized rigid joint i
with local coordinates φi .

Proof. First, consider an arbitrary (fixed) state (Q, p) of the system, with p =
M (Q)v. Then, by definition of a globally parameterized rigid joint, the functions
Fi (φi ) define local coordinates φi around Qi for all joints i. With Q fixed and
only φ changing, time derivatives of the configuration can locally be expressed
∂ i i
as ∂φ i Fi (φ )φ̇ (where, for joints with more than one DoF, summation over all

components of φi should take place). Joint velocities can be expressed using the
mappings V i for each joint, but instead of expressing them as functions of Qi
and Q̇i , we express them as functions of φi and φ̇i , as shown in (2.25). Since by
definition, V i is linear in Q̇, the expression V i can be written as a matrix-vector
product Si (Qi , φi )φ̇i as in (2.72). Collecting the matrices Si in the matrix S as
defined in (2.73), we obtain an expression for the velocities as S(Q, φ)φ̇.
Second, we can write the dynamic equations for the system locally around Q
in port-Hamiltonian form by using (2.62) with coordinates φ (and parameterized
by Q). The momentum can locally be written as

p := M (FQ (φ))S(Q, φ)φ̇ (2.76)

giving a Hamiltonian equal to 12 pT M −1 (FQ (φ))p + Up (Q). The Boltzmann-Hamel

44 CHAPTER 2. MODELING OF RIGID MECHANISMS

equations in port-Hamiltonian form then become

φ̇ = S −1 M −1 (FQ (φ))p (2.77)

 T T 
−T ∂H −T ∂ (S p) ∂(S T p)
ṗ = −S +S − S −1 M −1 (FQ (φ))p + u (2.78)
∂φ ∂φ ∂φ

where we abbreviated S = S(Q, φ) and where the partial derivative of H with

respect to φ should be taken as
!
∂H(Q, p) ∂H(Q, p) !! ∂Fi (φi )
i
:= i ! ◦ (2.79)
∂φ ∂Q Q=FQ (φ) ∂φi

with the concatenation operator (◦) meaning a sum over all elements of the matrix
Qi times the partial derivative of Fi (φi ) with respect to φi .
Finally, we obtain the dynamics at a certain point Q with pseudo-velocity v by
evaluating the equations at φ = 0. By definition, for φ = 0 we have Fi (0) = Qi
and S(Q, 0)φ̇ = v, which results in the dynamic equations (2.69)-(2.71) given in
the theorem. The conditions on Fi given in Definition 2.9 ensure the existence
and uniqueness of the partial derivatives of Fi and S T p with respect to φ. 
The equations in the theorem are basically just the Boltzmann-Hamel equa-
tions, but formulated in such a way that the local coordinates φ and φ̇ do not
appear (except in the partial derivatives of Si and Fi , evaluated at φ = 0). Thus,
the equations in the theorem are valid globally, for all Q, and they can be readily
implemented in simulation, by means of two ordinary differential equations for
the states Q and p plus an equation for the intermediate variable v.
Moreover, the structure of the mass matrix and the Jacobians can be used to
write the partial derivative of the Hamiltonian to φ in a compact form. This can
be seen from the definitions (2.40) and (2.59), which can be written as
⎡ 1 α T 1 ⎤
σi (X1 ) (Q ) AdTHαi

m
⎢ .. ⎥ i,i  1 
M= ⎣ . ⎦I σi AdHαi X1α (Q1 ) · · · σin AdHωi Xnω (Qn )
i=1
σin (Xnω )T (Qn ) AdTHωi
(2.80)

This shows that the mass matrix (and hence the Hamiltonian) only depends on
φi indirectly and locally, i.e. only through the matrix Xi (Qi ) defining the relative
degrees of freedom of joint i and through the homogeneous matrix Hi defining
the relative configuration of joint i. The partial derivatives of these terms to φi can
be easily obtained (looking only locally at the joint description), and hence the
partial derivative of the mass matrix follows by the chain rule for differentiation.
Example 2.18. As a first example, we consider the dynamics of a single rigid
body with frame Ψ0 , freely floating in space (we ignore gravity in this example).
2.3. DYNAMICS OF OPEN RIGID MECHANISMS 45

This system can be described as a (degenerate) mechanism with one rigid link
and one free 6-DoF joint as in Figure 2.4(e), using Q = H0∅ and v = T00,∅ . From
Appendix A.3, we find that the twist can be expressed locally around a certain Q
with exponential coordinates φ as T00,∅ = S(Q, φ)φ̇ with
∞
 (−1)k 1 1
S(Q, φ) = adk = I − adφ + ad2φ − . . . (2.81)
(k + 1)! φ 2 6
k=0

where ad is as defined in Lemma 2.8. From the expression, we can see that
S(Q, 0) = I, and we can evaluate the partial derivative with respect to φ at φ = 0
as follows.
!  !
∂(S T (Q, φ)p) !! ∂ 1 T 1 !
! = p − adφ p + (adφ ) p − . . . !!
2 T
∂φ φ=0 ∂φ 2 6 φ=0
      !
∂ I 0 pa 1 φ̃a φ̃b pa !
= + + . . . !!
∂φ 0 I pb 2 0 φ̃a pb φ=0
      !
∂ I 0 pa 1 p̃a p̃b φa !
= − + . . . !!
∂φ 0 I pb 2 p̃b 0 φb φ=0
 
1 p̃a p˜b
=− (2.82)
2 p̃b 0
where the subscripts a and b denote the first three and second three components
of a vector, respectively, and where the third equality follows from the identity

x̃y = x ∧ y = −y ∧ x = −ỹx (2.83)

which holds for all vectors x, y ∈ R3 . Since we used the twist in body coordinates
and ignored gravity, the inertia tensor I 0,0 and the Hamiltonian do not depend
on Q, and hence not on φ either. So if we substitute the expressions for S and its
partial derivative into Theorem 2.17, we obtain

Ḣ0∅ = H0∅ T̃00,∅ (2.84)

      
d pa 1 p̃Ta p̃Tb 1 p̃a p̃b
= − + T00,∅ + u
dt pb 2 p̃Tb 0 2 p̃b 0
 
p̃ p˜b 0,∅
= a T + W 0,0 (2.85)
p̃b 0 0

as the equations describing the dynamics of a rigid body in body-coordinates.

Instead of using a momentum in body coordinates, we can also use the mo-
mentum p∅ in world coordinates, defined as

p∅ := I ∅,∅ T0∅,∅ = AdTH 0 I 0,0 AdH∅0 AdH ∅ T00,∅ = AdTH 0 I 0,0 T00,∅ = AdTH 0 p (2.86)
∅ 0 ∅ ∅
46 CHAPTER 2. MODELING OF RIGID MECHANISMS

q2

q1
q3
q4

Figure 2.8: Example setup of a rigid mechanism for which Q is a vector.

Its time derivative can be obtained from the expression for ṗ as follows

d d
ṗ∅ = (AdTH 0 p) = (AdTH 0 )p + AdTH 0 ṗ = adTT ∅,0 AdTH 0 p + AdTH 0 ṗ
dt ∅ dt ∅ ∅ ∅ ∅
  0,∅ 
∅

d p̃ p̃b ω0
= (AdTH 0 )p + AdTH 0 a + AdTH 0 W 0,0
dt ∅ ∅ p̃b 0 v00,∅ ∅

  
d ω̃ 0,∅ ṽ00,∅ pa
= (AdTH 0 )p − AdTH 0 0 + W ∅,0
dt ∅ ∅ 0 ω̃00,∅ pb
d
= (AdTH 0 ) AdTH ∅ p∅ + AdTH 0 adTT 0,∅ AdTH ∅ p∅ + W ∅,0
dt ∅ 0 ∅ 0 0

d d
= (AdTH 0 ) AdTH ∅ p∅ + AdTH 0 (AdTH ∅ )p∅ + W ∅,0
dt

∅ 0 ∅ dt 0

d d
= (AdTH 0 ) AdTH ∅ + AdTH 0 (AdTH ∅ ) p∅ + W ∅,0
dt ∅ 0 ∅ dt 0

= W ∅,0 (2.87)

where we used property (f) of Lemma 2.8 and where the last equality follows
from (2.68). The resulting equation is a generalization of Newton’s second law
to the case of a rigid body: the time-derivative of the momentum of a rigid body
(when expressed in inertial coordinates) is equal to the forces (wrenches) applied
to it, and hence the momentum is conserved if no external forces are applied. 

Example 2.19. As a second example of the general result of Theorem 2.17, we

consider the dynamics of a class of mechanisms for which the allowed relative
configuration Qi of all joints can be described by a vector, such as the mechanism
shown in Figure 2.8, with full direct actuation τ . In this case, we can choose Q = q
and v = q̇, as well as FQ (φ) = Q + φ, and hence we obtain S(Q, φ) = I and the
2.4. KINEMATIC LOOPS AND NONHOLONOMIC CONSTRAINTS 47

corresponding dynamics
     ∂H   
d q 0 I ∂q 0
= + τ (2.88)
dt p −I 0 ∂H
∂p
I

with H(q, p) = 12 pT M −1 (q)p + Up (q), i.e. the normal Euler-Lagrange equations in

port-Hamiltonian form. The equation for ṗ can be expanded as

d ∂(M (q)q̇)
ṗ = (M (q)q̇) = q̇ + M (q)q̈
dt ∂q
∂H 1 ∂(q̇ T M (q)q̇) ∂Up
=− +τ = − +τ (2.89)
∂q 2 ∂q ∂q
which can be written as
 
∂(M (q)q̇) 1 ∂ T (M (q)q̇) ∂Up
M (q)q̈ + − q̇ +
∂q i 2 ∂q ∂q
=: M (q)q̈ + C(q, q̇)q̇ + V (q) = τ (2.90)

which is the formulation of the dynamics of this class of mechanisms as com-

monly encountered in textbooks. The matrix C(q, q̇) describes the centrifugal and
Coriolis effects. 

2.4 Kinematic loops and nonholonomic constraints

The previous sections have shown how to obtain a set of explicit differential equa-
tions for general mechanisms with ideal holonomic joints and a loop-free kine-
matic graph, i.e. for systems without kinematic loops and for which the space of
allowed configurations has the same dimension as the space of allowed instanta-
neous velocities. In this section, we look at more general mechanisms, possibly
containing kinematic loops and nonholonomic constraints.
For conciseness reasons, we choose to start here from systems described by co-
ordinates q ∈ Rn and p ∈ Rn , with Hamiltonian equal to H(q, p) = 12 pT M −1 (q)p+
Up (q) and described by the differential equations (2.61). The results from this sec-
tion can be extended to more complex descriptions such as in Theorem 2.17; the
equations just become a little longer.
We consider constraints that can be expressed in the form

AT (q)q̇ = 0 (2.91)

with A(q) a continuously differentiable n × m matrix. Furthermore, the associ-

ated constraint forces λ are collocated with AT q̇. This formulation means that
48 CHAPTER 2. MODELING OF RIGID MECHANISMS

we consider position-varying velocity constraints that are linear in the velocities

with constraint forces that do not influence the energy of the system (since the
power equals λT (AT q̇) = 0 if the constraints are satisfied).
Under these conditions, the mechanical system with constraints can be repre-
sented by the implicit set of differential equations
     ∂H    
d q 0 I ∂q 0 0 λ
= ∂H + A(q) B(q)
dt p −I 0 ∂p
u
   
 ∂H  (2.92)
0 0 AT (q) ∂q
=
y 0 B T (q) ∂H ∂p

where λ is to be determined such that the output zero constraint is and remains
satisfied.
Several approaches can be taken in the simulation of systems of the form
(2.92). First, it could be left to a numerical solver to obtain the correct λ at each
simulation step. We do not further pursue this direction, but refer to Ascher &
Petzold (1998) for an introduction to numerical techniques for dealing with these
sets of algebraic and differential equations.
A second approach is to obtain λ by differentiation of the constraint:
 
d T ∂H d  T −1  ∂(AT M −1 p)
0= A = A M p = q̇ + AT M −1 ṗ
dt ∂p dt ∂q
 
∂(AT M −1 p) ∂H ∂H
= + AT M −1 − + Aλ + Bu (2.93)
∂q ∂p ∂q

from which λ can be solved as

 T −1  ∂H ∂(AT M −1 p) ∂H
A M A λ = AT M −1 − AT M −1 Bu − (2.94)
∂q ∂q ∂p
which has a unique solution for λ if the matrix A has full column rank. This
solution gives the necessary force λ that keeps the velocities AT q̇ constant (since
we looked at the derivative of the constraint). This means that if the constraint
(2.91) is satisfied at some time, then applying this λ ensures that (2.91) remains
satisfied. The approach can be extended to avoid numerical drift (which may
cause AT q̇ to become nonzero), by changing the differential equation as follows
     
d T ∂H d T ∂H T ∂H
A =0 → A = −β A (2.95)
dt ∂p dt ∂p ∂p

with β > 0 a damping coefficient. We apply this method in Section 3.3, where
we discuss the computation of contact forces for rigid contact between two rigid
bodies.
2.4. KINEMATIC LOOPS AND NONHOLONOMIC CONSTRAINTS 49

A third approach (presented by Duindam, Blankenstein & Stramigioli (2004),

based on results by van der Schaft & Maschke (1994)) is to find new coordinates
for the momentum such that satisfying the constraints is equivalent to some of
these coordinates being zero. This approach can be taken if the rank of A is
constant (say, m) for all q, by defining an n × (n − m) matrix S(q) satisfying
AT (q)M −1 (q)S(q) = 0, i.e. such that the columns of S span the space of mo-
menta compatible with the constraints. The dynamics can then be described by
the explicit differential equations of the following theorem.

Theorem 2.20 (Dynamics of Nonholonomic Systems). Given a system with

nonholonomic constraints, described by (2.92) and let A(q) have constant rank m
for all q. If we can find a continuously differentiable n × (n − m) matrix S(q)
satisfying AT (q)M −1 (q)S(q) = 0, then the dynamics of the constrained system can
be described equivalently by the following port-Hamiltonian system
     
∂ H̄
d q ¯ α) ∂q + 0
= J(q, u
dt α ∂ H̄ M̄ S T M −1
∂α
  (2.96)
  ∂ H̄
y = 0 M −1 S M̄ ∂∂q H̄
∂α

with α an (n − m)-vector, and M̄ , H̄, and J¯ defined as

 −1
M̄ (q) := S T (q)M −1 (q)S(q) (2.97)
1
H̄(q, α) := H(q, S(q)α) = αT M̄ −1 α + V (q) (2.98)
 2 
−1
0  TM S M̄ 
¯
J(q, α) := (Sα) (2.99)
−M̄ S T M −1 M̄ S T M −1 ∂ ∂q − ∂(Sα)
∂q M −1 S M̄

Proof. The vector α is the new reduced-order coordinate vector for the momen-
tum satisfying p = S(q)α. By definition of S(q), the constraints 0 = AT M −1 p =
AT M −1 Sα are automatically satisfied for all α. Furthermore, the definition of the
new Hamiltonian H̄ is just H with S(q)α substituted for p. From this definition
we find

∂ H̄ ∂H ∂ T (S(q)α) ∂H
= + (2.100)
∂q ∂q ∂q ∂p
∂ H̄ ∂H
= S T (q) (2.101)
∂α ∂p
50 CHAPTER 2. MODELING OF RIGID MECHANISMS

SH T
SN
∂H ∂ H̄
q̇ v= ∂p ∂α
−1
SH

−1 −T
SH M −1 SH T
SH M SH M −1 M M̄ −1 M̄

−T
SH
T SN
SH p p α
T
SH

Figure 2.9: Commutation diagram for a system with position coordinates q,

pseudo-velocities v, and momenta α allowed by the nonholonomic constraints.

Furthermore, we can write the differential equations for q and p as

∂H ∂ H̄
q̇ = = M −1 p = M −1 Sα = M −1 S M̄ M̄ −1 α = M −1 S M̄ (2.102)
∂p ∂α
d ∂(Sα) ∂H
ṗ = (S(q)α) = q̇ + S α̇ = − + Aλ + Bu (2.103)
dt ∂q ∂q

Pre-multiplying the last equation with M̄ S T M −1 gives

∂(Sα) −1 ∂ H̄
M̄ S T M −1 M S M̄ + α̇
∂q ∂α
 
∂ H̄ ∂ T (Sα) −1
= −M̄ S T M −1 − M Sα + M̄ S T
" M
−1 T −1
#$ A% λ + M̄ S M Bu
∂q ∂q
0
 T

∂ H̄ ∂ (Sα)
= −M̄ S T M −1 − M −1 Sα (2.104)
∂q ∂q

which, together with the equation for q̇, can be formulated as in the theorem. 
The results of Theorem 2.20 are an extension of the Boltzmann-Hamel equa-
tions (2.62), with the S −1 replaced by a pseudo-inverse M −1 S(S T M −1 S)−1 . How-
ever, the mass matrix M of the system without kinematic constraints is necessary
for the definition of the new system in reduced coordinates, and hence it is useful
to distinguish the holonomic joints (where S is invertible) from the nonholonomic
joints (where S is not invertible).
To illustrate the precise difference between the Boltzmann-Hamel equations
of Lemma 2.16 and the equations for nonholonomic systems of Theorem 2.20,
consider a combination of both systems, i.e. a system with coordinates q, pseudo-
velocities v = SH (q)q̇ (with SH invertible), corresponding momentum variables
2.4. KINEMATIC LOOPS AND NONHOLONOMIC CONSTRAINTS 51

1 1
2 mw , 2 Jw

r
φ
ψ
θ
r
mb , Jb mt , Jt
φ y
1 1
2 mw , 2 Jw

Figure 2.10: Schematic (top) view of the snakeboard.

p = M v, and nonholonomic constraints expressed by reduced-order coordinates

α satisfying p = SN (q)α (with SN not invertible).
The relation between the various variables is illustrated in Figure 2.9. Apart
from the relations mentioned above, the definitions of H(q, p) and H̄(q, α) allow
to construct the other relations in the diagram. From the diagram, we can see that
−1
q̇ can be obtained from ∂H∂p just by multiplication with SH . However, since SN
T

is not invertible, v can be obtained from ∂∂α

H̄
by going via the variables α and p,
−1 ∂ H̄
i.e. as v = M SN M̄ ∂α . This illustrates the difference between Lemma 2.16 and
Theorem 2.20.
Example 2.21. Figure 2.10 shows the snakeboard, an interesting mechanical sys-
tem with nonholonomic constraints. It is a commercially available locomotion
device (Snakeboard U.S.A. 2005), very similar to the skateboard, but with extra
degrees of freedom that allow rotation of the front and rear wheel bases around
the vertical axis, such that the direction of motion of the wheels can be changed.
Using certain combinations of motion of the feet (to control the wheel base an-
gles) and of the upper body, the person riding the snakeboard can increase the
total forward momentum, without touching the ground or directly driving the
wheels.
The snakeboard is a relatively simple system that still has many scientifically
interesting properties, and it is hence used in numerous studies on modeling,
analysis, and control of nonholonomic mechanical systems. Lewis et al. (1994)
and Bullo & Zefran (2001) used Lie-bracket analysis, Vela (2003) applied averag-
ing analysis, and Ostrowski & Burdick (1998), Bloch (2003), Bloch et al. (1996),
Ostrowski (1999), and Blankenstein (2003) used principle bundles and momen-
tum maps. These studies have revealed many interesting physical and geometri-
cal properties and interpretations, and provide general tools to analyze nonholo-
52 CHAPTER 2. MODELING OF RIGID MECHANISMS

nomic systems.
In this example, we show how an explicit model of the snakeboard can be
obtained by choosing new coordinates for the momentum, as described by The-
orem 2.20. The advantage of this formulation is not only the possibility of fast
simulation using straight-forward ODE algorithms, but also that the resulting
equations turn out to be very simple and allow clear interpretation in terms of
energy flows. More information about the use of this model for controller design
can be found in (Duindam & Stramigioli 2004a) and (Duindam, Blankenstein &
Stramigioli 2004).
To construct a model of the snakeboard, we assume that it consists of three
rigid parts (body, torso and wheels), each with its own mass m∗ and inertia J∗ .
We can write the total mass and inertia as follows
m := mb + mt + mw total mass
2
J := Jb + Jt + Jw + mw r total inertia around (x, y)
and we make the simplifying assumptions that (i) the wheels rotate at equal but
opposite angles (only one coordinate φ is used) and (ii) the parameters are such
that J = mr2 . These assumptions are standard in the analysis of the snakeboard
and simplify the equations without affecting the essential geometry of the prob-
lem. We choose the states of the system as positions and momenta
 T
q= x y θ ψ φ
 T (2.105)
p = px py pθ pψ pφ

and the Hamiltonian of the system is just the kinetic energy H = 12 pT M −1 p with
⎡ ⎤
m 0 0 0 0
⎢0 m 0 0 0⎥
⎢ ⎥
⎢
M = ⎢ 0 0 J Jt 0 ⎥ ⎥ (2.106)
⎣ 0 0 Jt Jt 0 ⎦
0 0 0 0 Jw
We furthermore assume to have direct torque control on the torso and the wheels,
which can be described by the input mapping
 T
0 0 0 1 0
B= (2.107)
0 0 0 0 1
Finally, the (nonholonomic) kinematic constraints are that the wheels are not al-
lowed to slip sideways. This can be represented by the matrix A as
 T
sin(θ − φ) − cos(θ − φ) r cos(φ) 0 0
A= (2.108)
sin(θ + φ) − cos(θ + φ) −r cos(φ) 0 0
2.4. KINEMATIC LOOPS AND NONHOLONOMIC CONSTRAINTS 53

which has rank 2 for all q, except for φ = ± 12 π, i.e. when the wheels are turned
completely sideways. The two elements of AT q̇ describe the sideways velocities
of the two wheel pairs (front and rear).
We can find an example of a suitable matrix S(q) describing the allowed mo-
menta as follows
⎡ mr mr mr 2 −Jt
⎤T
β cos(θ) cos(φ) β sin(θ) cos(φ) β sin(φ) 0 0
S(q) = ⎣ 0 0 1 1 0⎦ (2.109)
0 0 0 0 1
&
with β(φ) := mr2 − Jt sin2 (φ). Duindam, Blankenstein & Stramigioli (2004)
show how different choices of S(q) lead to the different models available in liter-
ature. If we use this matrix S(q) in Theorem 2.20, we obtain the following explicit
differential equations that describe the dynamics of the constrained snakeboard.
⎡r ⎤
cos(θ) cos(φ) 0 0
β ⎡ ⎤
⎢ βr sin(θ) cos(φ) 0 0⎥ α1
d ⎢ ⎥
q=⎢ ⎢
1
β sin(φ) 0 0⎥ ⎣ αJ2 ⎦
⎥ αt (2.110)
dt ⎣ − 1 sin(φ) ⎦ 3
β 1 0 Jw
0 0 1
⎡1 ⎤
β sin(φ) 0
 
d ⎣ ⎦ u1
α= 1 0 (2.111)
dt u 2
0 1
 
with H = 12 αT M̄ −1 α and M̄ = diag 1 Jt Jw . The mass matrix M̄ is constant
and diagonal, which is due to the choice of S: the matrix M −1 defines a metric on
the space of momenta, and the columns of S are chosen to be mutually orthogonal
and of constant norm in that metric.
The resulting dynamic equations turn out to be very simple, expressing the
total kinetic energy of the system as the sum of three terms, each depending on
only one momentum coordinate: α3 represents the energy in the wheels, α2 the
energy stored in the torso, and α1 the energy stored in the other motions of the
snakeboard, in particular the forward momentum of the board (a quantity that
may be important for control).
Since the energy function depends only on α and not on q, we can represent
the energy flows in the snakeboard as in Figure 2.11 in terms of a bond graph
(bond graphs are explained in detail in Appendix B.2). From the bond graph,
we see that the wheels do not have an energy coupling to the rest of the snake-
board; they only influence the system indirectly through modulation of the MTF.
Furthermore, the input port (u1 ,ψ̇) can exchange energy with the torso (I element
with parameter Jt ) as well as the forward motions (I element with parameter 1),
but only if sin(φ) = 0. All of this is clear from intuitive physical reasoning about
54 CHAPTER 2. MODELING OF RIGID MECHANISMS

u2
I : Jw
φ̇
φ

MTF 1 I :1
1
β(φ)
u1
0 I : Jt
ψ̇
Figure 2.11: Bond graph of the snakeboard in reduced momentum coordinates.

the snakeboard (e.g. if the wheels are straight (φ = 0), exerting a torque u1 only
makes the torso rotate, not the rest of the snakeboard); the reduced equations and
the corresponding bond graph prove this reasoning and quantify it.