Advanced Mechanics Concepts

Newton-Euler mechanics has the advantage that it starts with very basic prin- ciples, but it has frustrating restrictions that make modeling more difficult for complicated mechanical systems. One of the main limitations is that all laws must be expressed in terms of an inertial frame with orthogonal axes. This section in- troduces the basic ideas of Lagrangian and Hamiltonian mechanics, which remove these restrictions by reducing mechanics to finding an optimal path using any coor- dinate neighborhood of the C-space. The optimality criterion is expressed in terms of energy. The resulting techniques can be applied on any coordinate neighborhood of a smooth manifold. The Lagrangian formulation is usually best for determining the motions of one or more bodies. Section 13.4.1 introduces the basic Lagrangian concepts based on the calculus of variations. Section 13.4.2 presents a general form of the Euler-Lagrange equations, which is useful for determining the motions of numerous dynamical systems, including chains of bodies. The Lagrangian is also convenient for systems that involve additional differential constraints, such as friction or rolling wheels. These cases are briefly covered in Section 13.4.3. The Hamiltonian formulation in Section 13.4.4 is based on a special phase space and provides an alternative to the Lagrangian formulation. The technique generalizes to Pontryagin’s minimum principle, a powerful optimal control technique that is covered in Section 15.2.3.

• • 1. Lagrangian Mechanics
       1. Calculus of variations

Lagrangian mechanics is based on the calculus of variations, which is the subject of optimization over a space of paths. One of the most famous variational problems involves constraining a particle to travel along a curve (imagine that the particle slides along a frictionless track). The problem is to find the curve for which the ball travels from one point to the other, starting at rest, and being accelerated only by gravity. The solution is a cycloid function called the Brachistochrone curve [841]. Before this problem is described further, recall the classical optimization problem from calculus in which the task is to find extremal values (minima and

x(t)

t

Figure 13.12: The variation is a “small” function that is added to x˜ to perturb it.

maxima) of a function. Let x˜ denote a smooth function from R to R, and let x(t) denote its value for any t ∈ R. From standard calculus, the extremal values of x˜

are all t ∈ R for which x˙ = 0. Suppose that at some t′ ∈ R, x˜ achieves a local

minimum. To serve as a local minimum, tiny perturbations of t′ should result in

larger function values. Thus, there exists some d > 0 such that x(t′ + ǫ) > x(t′) for any ǫ ∈ [−d, d]. Each ǫ represents a possible perturbation of t′.

The calculus of variations addresses a harder problem in which optimization occurs over a space of functions. For each function, a value is assigned by a criterion called a functional.10 A procedure analogous to taking the derivative of the function and setting it to zero will be performed. This will be arrived at by considering tiny perturbations of an entire function, as opposed to the ǫ perturbations mentioned above. Each perturbation is itself a function, which is called a variation. For a function to minimize a functional, any small enough perturbation of it must yield a larger functional value. In the case of optimizing a function of one variable, there are only two directions for the perturbation: ǫ. See Figure 13.12. In the calculus of variations, there are many different “directions” because of the uncountably infinite number of ways to construct a small variation function that perturbs the original function (the set of all variations is an infinite- dimensional function space; recall Example 8.5).

±

Let x˜

denote a smooth function from T = [t0, t1] into R. The functional is

defined by integrating a function over the domain of x˜. Let L be a smooth, real-

valued function of three variables, a, b, and c.11 The arguments of L may be any a, b R and c T to yield L(a, b, c), but each has a special interpretation. For some smooth function x˜, L is used to evaluate it at a particular t T to obtain L(x, x˙ , t). A functional Φ is constructed using L to evaluate the whole function x˜

∈

∈ ∈

10This is the reason why a cost functional has been used throughout the book. It is a function on a space of functions.

11Unfortunately, L is used here to represent a cost function, on which a functional Φ will be based. This conflicts with using l as a cost function and L as the functional in motion planning formulations. This notational collision remains because L is standard notation for the Lagrangian. Be careful to avoid confusion.

as

r

Φ(x˜) =

T

L(x(t), x˙ (t), t)dt. (13.114)

The problem is to select an x˜ that optimizes Φ. The approach is to take the

derivative of Φ and set it equal to zero, just as in standard calculus; however, differentiating Φ with respect to x˜ is not standard calculus. This usually requires special conditions on the class of possible functions (e.g., smoothness) and on the vector space of variations, which are implicitly assumed to hold for the problems considered in this section.

Example 13.9 (Shortest-Path Functional) As an example of a functional, consider

√

L(x, x˙ , t) = 1 + x˙ 2. (13.115)

When evaluated on a function x˜, this yields the arc length of the path. ．

Another example of a functional has already been seen in the context of motion planning. The cost functional (8.39) assigns a cost to a path taken through the state space. This provided a natural way to formulate optimal path planning. A discrete, approximate version was given by (7.26).

Let h be a smooth function over T , and let ǫ R be a small constant. Consider the function defined as x(t)+ ǫh(t) for all t [0, 1]. If ǫ = 0, then (13.114) remains

∈

∈

the same. As ǫ is increased or decreased, then Φ(x˜+ǫh) may change. The function h is like the “direction” in a directional derivative. If for any smooth function h, their exists some ǫ > 0 such that the value Φ(x˜ + ǫh) increases, then x˜ is called an extremal of Φ. Any small perturbation to x˜ causes the value of Φ to increase. Therefore, x˜ behaves like a local minimum in a standard optimization problem.

Let g = ǫh for some ǫ > 0 and function h. The differential of a functional can be approximated as [39]

Φ(x˜ + g) − Φ(x˜) = r

= r

T

(L(x(t) + g(t), x˙ (t) + g˙ (t), t) − L(x(t), x˙ (t), t)\dt + · · ·

（ ∂Lg + ∂Lg˙ ＼ dt + · · ·

T ∂x ∂x˙

r （ ∂L d ∂L ＼

−

（ ∂L

＼ I_t_1

= g g

T ∂x dt ∂x˙

dt +

g

∂x˙

IIII_t_0

• · · · ,

(13.116)

in which represents higher order terms that will vanish in the limit. The last step follows from integration by parts:

· · ·

∂Lg

（

∂x˙

_t_1

IIIt_0 _T

＼ II

r

=

∂L

g˙ dt +

r

∂x˙ T

d ∂L

hdt, (13.117)

dt ∂x˙

which is just uv = vdu + udv. Consider the value of (13.116) as ǫ becomes small, and assume that h(t0) = h(t1) = 0. For x˜ to be an extremal function, the change expressed in (13.116) should tend to zero as the variations approach zero. Based on further technical assumptions, including the Fundamental Lemma of the Calculus of Variations (see Section 12 of [39]), the Euler-Lagrange equation,

r r

d ∂L dt ∂x˙

∂L

− ∂x = 0, (13.118)

is obtained as a necessary condition for x˜ to be an extremum. Intuition can be gained by studying the last line of (13.116). The integral attains a zero value pre- cisely when (13.118) is satisfied. The other terms vanish because h(t0) = h(t1) = 0, and higher order terms disappear in the limit process.

The partial derivatives of L with respect to x˙ and x are defined using standard

calculus. The derivative ∂L/∂x˙

is evaluated by treating x˙

as an ordinary variable

(i.e., as ∂L/∂b when the variables are named as in L(a, b, c)). Following this, the

derivative of ∂L/∂x˙ with respect to t is taken. To illustrate this process, consider

the following example.

Example 13.10 (A Simple Variational Problem) Let L be a functional de- fined as

L(x, x˙ , t) = x3 + x˙ 2. (13.119)

The partial derivatives with respect to x and x˙ are

∂L

= 3x2 (13.120)

∂x

and

∂L

∂x˙

= 2x˙ . (13.121)

Taking the time derivative of (13.121) yields

d ∂L dt ∂x˙

= 2x¨

(13.122)

Substituting these into the Euler-Lagrange equation (13.118) yields

d ∂L dt ∂x˙

∂L 2

− ∂x = 2x¨ − 3x

= 0. (13.123)

This represents a second-order differential constraint that constrains the acceler-

ation as x¨ = 3x2/2. By constructing a 2D phase space, the constraint could be expressed using first-order differential equations. ．

• • • 1. Hamilton’s principle of least action

Now sufficient background has been given to return to the dynamics of mechanical systems. The path through the C-space of a system of bodies can be expressed as the solution to a calculus of variations problem that optimizes the difference be-

tween kinetic and potential energy. The calculus of variations principles generalize to any coordinate neighborhood of C. In this case, the Euler-Lagrange equation is

d ∂L

dt ∂q˙ −

∂L

= 0, (13.124)

∂q

in which q is a vector of n coordinates. It is actually n scalar equations of the form

d ∂L dt ∂q˙i

∂L

− ∂qi

= 0. (13.125)

The coming presentation will use (13.124) to obtain a phase transition equation. This will be derived by optimizing a functional defined as the change in kinetic and potential energy. Kinetic energy for particles and rigid bodies was defined in Section 13.3.1. In general, the kinetic energy function must be a quadratic function of q˙. Its definition can be interpreted as an inner product on , which causes to become a Riemannian manifold [156]. This gives the manifold a notion of the “angle” between velocity vectors and leads to well-defined notions of curvature and shortest paths called geodesics. Let K(q, q˙) denote the kinetic energy, expressed using the manifold coordinates, which always takes the form

C C

K(q, q˙) = 1 q˙T M(q)q˙, (13.126) in which M(q) is an n n matrix called the mass matrix or inertia matrix.

2

×

The next step is to define potential energy. A system is called conservative

if the forces acting on a point depend only on the point’s location, and the work done by the force along a path depends only on the endpoints of the path. The total energy is conserved under the motion of a conservative system. In this case, there exists a potential function φ : W R such that F = ∂φ/∂p, for any p W . Let V (q) denote the total potential energy of a collection of bodies, placed at configuration q.

→ ∈

It will be assumed that the dynamics are time-invariant. Hamilton’s principle

of least action states that the trajectory, coincides with extremals of the functional,

q˜ : T → C, of a mechanical system

Φ(q˜) =

r

T

(K(q(t), q˙(t)) − V (q(t))\dt, (13.127)

using any coordinate neighborhood of . The principle can be seen for the case of

C

3

C = R by expressing Newton’s second law in a way that looks like (13.124) [39]:

d

−

(mq˙)

dt

∂V

= 0, (13.128)

∂q

in which the force is replaced by the derivative of potential energy. This suggests applying the Euler-Lagrange equation to the functional

L(q, q˙) = K(q, q˙) − V (q), (13.129)

in which it has been assumed that the dynamics are time-invariant; hence, L(q, q˙, t) =

L(q, q˙). Applying the Euler-Lagrange equation to (13.127) yields the extremals.

The advantage of the Lagrangian formulation is that the C-space does not have to be = R3, described in an inertial frame. The Euler-Lagrange equation gives a necessary condition for the motions in any C-space of a mechanical system.

C

The conditions can be expressed in terms of any coordinate neighborhood, as op- posed to orthogonal coordinate systems, which are required by the Newton-Euler formulation. In mechanics literature, the q variables are often referred to as gen- eralized coordinates. This simply means the coordinates given by any coordinate neighborhood of a smooth manifold.

Thus, the special form of (13.124) that uses (13.129) yields the appropriate constraints on the motion:

d ∂L ∂L

−

d ∂K(q, q˙)

∂K(q, q˙)

∂V (q)

dt ∂q˙

=

∂q dt

∂q˙

• = 0. (13.130)

∂q ∂q

Recall that this represents n equations, one for each coordinate qi. Since K(q, q˙) does not depend on time, the d/dt operator simply replaces q˙ by q¨ in the calculated expression for ∂K(q, q˙)/∂q˙. The appearance of q¨ seems appropriate because the resulting differential equations are second-order, which is consistent with Newton- Euler mechanics.

−

Example 13.11 (A Falling Particle) Suppose that a particle with mass m is falling in R3. Let (q1, q2, q3) denote the position of the particle. Let g denote

the acceleration constant of gravity in the −q3 direction. The potential energy is

V (q) = mgq3. The kinetic energy is

K(q, q˙) = 1 mq˙ · q˙ = 1 m(q˙2 + q˙2 + q˙2). (13.131)

The Lagrangian is

2 2 1 2 3

L(q, q˙) = K(q, q˙) − V (q) = m(q˙ + q˙ + q˙ ) − mgq3 = 0. (13.132)

1 2 2 2

2

1

2

3

To obtain the differential constraints on the motion of the particle, use (13.130).

For each i from 1 to 3,

d ∂L

dt ∂q˙

d

= (mq˙i) = mq¨i (13.133)

dt

Since K(q, q˙) does not depend on q, the derivative ∂K/∂qi = 0 for each i. The derivatives with respect to potential energy are

∂V

= 0

∂q1

∂V

= 0

∂q2

∂V

∂q3

= mg. (13.134)

Substitution into (13.130) yields three equations:

mq¨1 = 0 mq¨2 = 0 mq¨3 + mg = 0. (13.135)

These indicate that acceleration only occurs in the q3 direction, and this is due to gravity. The equations are consistent with Newton’s laws. As usual, a six- dimensional phase space can be defined to obtain first-order differential constraints.

−

．

The “least” part of Hamilton’s principle is actually a misnomer. It is technically only a principle of “extremal” action because (13.130) can also yield motions that maximize the functional.

• • • 1. Applying actions

Up to this point, it has been assumed that no actions are applied to the mechanical system. This is the way the Euler-Lagrange equation usually appears in physics

because the goal is to predict motion, rather than control it. Let u Rn denote an action vector. Actions can be applied to the Lagrangian formulation as generalized

∈

forces that “act” on the right side of the Euler-Lagrange equation. This results in

d ∂L

dt ∂q˙ −

∂L

= u. (13.136)

∂q

The actions force the mechanical system to deviate from its usual behavior. In some instances, the true actions may be expressed in terms of other variables, and then u is obtained by a transformation (recall transforming action variables for the differential drive vehicle of Section 13.1.2). In this case, u may be replaced in (13.136) by φ(u) for some transformation φ. In this case, the dimension of u need not be n.

• • • 1. Procedure for deriving the state transition equation

The following general procedure can be followed to derive the differential model us- ing Lagrangian mechanics on a coordinate neighborhood of a smooth n-dimensional manifold:

• • • • 1. Determine the degrees of freedom of the system and define the appropriate

n-dimensional smooth manifold C.

• • • • 1. Express the kinetic energy as a quadratic form in the configuration velocity components:

1 1 n n

K(q, q˙) =

q˙T M(q)q˙ =

2

2 - - mij (q)q˙i_q˙_j . (13.137)

i=1 j=1

• • • • 1. Express the potential energy V (q).
        2. Let L(q, q˙) = K(q, q˙) V (q) be the Lagrangian function, and use the Euler- Lagrange equation (13.130) to determine the differential constraints.

−

• • • • 1. Convert to phase space form by letting x = (q, q˙). If possible, solve for x˙ to obtain x˙ = f(x, u).

Example 13.12 (2D Rigid Body Revisited) The equations in (13.109) can be alternatively derived using the Euler-Lagrange equation. Let C = R2 × S1, and

let (q1, q2, q3) = (x, y, θ) to conform to the notation used to express the Lagrangian.

The kinetic energy is the sum of kinetic energies due to linear and angular velocities, respectively. This yields

K(q, q˙) = 1 mq˙ · q˙ + 1 Iq˙2, (13.138)

2

2

3

in which m and I are the mass and moment of inertia, respectively. Assume there is no gravity; hence, V (q) = 0 and L(q, q˙) = K(q, q˙).

Suppose that generalized forces u1, u2, and u3 can be applied to the configu- ration variables. Applying the Euler-Lagrange equation to L(q, q˙) yields

d ∂L ∂L d

dt ∂q˙1

− ∂q1

= (mq˙1) = mq¨1 = u1 dt

d ∂L ∂L d

dt ∂q˙2

− ∂q2

= (mq˙2) = mq¨2 = u2 dt

(13.139)

d ∂L ∂L d

dt ∂q˙3

− ∂q3

= (Iq˙3) = Iq¨3 = u3. dt

These expressions are equivalent to those given in (13.109). One difference is that conversion to the phase space is needed. The second difference is that the action variables in (13.139) do not refer directly to forces and moments. They are in- stead interpreted as generalized forces that act on the configuration variables. A

conversion should be performed if the original actions in (13.109) are required. ．

• • 1. General Lagrangian Expressions

As more complicated mechanics problems are considered, it is convenient to express the differential constraints in a general form. For example, evaluating (13.130) for a kinematic chain of bodies leads to very complicated expressions. The terms of these expressions, however, can be organized into standard forms that appear simpler and give some intuitive meanings to the components.

Suppose that the kinetic energy is expressed using (13.126), and let mij (q) denote an entry of M(q). Suppose that the potential energy is V (q). By per- forming the derivatives expressed in (13.136), the Euler-Lagrange equation can be

expressed as n scalar equations of the form [856]

n n n

• mij (q)q¨j + - - hijk(q)q˙j q˙k + gi(q) = ui (13.140)

in which

j=1

j=1 k=1

∂mij 1 ∂mjk

h = − . (13.141)

ijk

∂qk

2 ∂qi

There is one equation for each i from 1 to n. The components of (13.140) have physical interpretations. The mii coefficients represent the inertia with respect

to qi. The mij represent the affect on qj of accelerating qi. The hijj q˙2

j

terms

represent the centrifugal effect induced on qi by the velocity of qj . The hijk_q˙_j q˙k terms represent the Coriolis effect induced on qi by the velocities of qj and qk. The gi term usually arises from gravity.

An alternative to (13.140) is often given in terms of matrices. It can be shown that the Euler-Lagrange equation reduces to

M(q)q¨ + C(q, q˙)q˙ + g(q) = u, (13.142) which represents n scalar equations. This introduces C(q, q˙), which is an n n

×

Coriolis matrix. It turns out that many possible Coriolis matrices may produce

equivalent different constraints. With respect to (13.140), the Coriolis matrix must be chosen so that

n n n

• cij q˙j = - - hijk_q˙_j q˙k. (13.143)

Using (13.141),

j=1

j=1 k=1

n n n

• c q˙ = - - （ ∂mij − 1 ∂mjk ＼ q˙ q˙ . (13.144)

j=1

j=1 k=1

k

i

j=1

j=1 k=1

k

i

ij

j

∂q

2

∂q

j

k

A standard way to determine C(q, q˙) is by computing Christoffel symbols. By subtracting 1 ∂mjk from the inside of the nested sums in (13.144), the equation can

2 ∂qi

be rewritten as

n n n n n

k

2

∂q

∂q

j

k

• c q˙

• -

ij

j=1

1 ∂mij

= q˙ q˙

j

2

∂q

j

j=1 k=1

k

• 1 - - （ ∂mij − ∂mjk ＼ q˙ q˙ . (13.145)

j=1 k=1

k

i

j=1

j=1 k=1

k

j=1 k=1

k

i

This enables an element of C(q, q˙) to be written as

in which

n

cij = cijk_q˙_k, (13.146)

-

k=1

c = 1 （ ∂mij + ∂mik − ∂mjk ＼ . (13.147)

ijk

2

∂q

∂q

∂q

k

j

i

k

j

i

Figure 13.13: Parameter values for a two-link robot with two revolute joints.

This is called a Christoffel symbol, and it is obtained from (13.145). Note that

cijk = cikj . Christoffel symbols arise in the study of affine connections in differential

geometry and are usually denoted as Γi

jk

. Affine connections provide a way to

express acceleration without coordinates, in the same way that the tangent space was expressed without coordinates in Section 8.3.2. For affine connections in differential geometry, see [133]; for their application to mechanics, see [156].

• • • 1. Conversion to a phase transition equation

The final step is to convert the equations into phase space form. A 2n-dimensional phase vector is introduced as x = (q, q˙). The task is to obtain x˙ = f(x, u), which represents 2n scalar equations. The first n equations are x˙ i = xn+i for i from 1 to

1. The final n equations are obtained by solving for q¨.

Suppose that the general form in (13.142) is used. Solving for q¨ yields

q¨ = M(q)−1(u − C(q, q˙)q˙ − g(q)). (13.148) The phase variables are then substituted in a straightforward manner. Each q¨i for

i from 1 to n becomes x˙ n+i, and M(q), C(q, q˙), and g(q) are expressed in terms of

x. This completes the specification of the phase transition equation.

Example 13.13 (Two-Link Manipulator) Figure 13.13 shows a two-link ma- nipulator for which there are two revolute joints and two links, A1 and A2. Hence,

1

C = S1 × S . Let q = (θ1, θ2) denote a configuration. Each of the two joints is

controlled by a motor that applies a torque ui. Let u1 apply to the base, and let

u2 apply to the joint between A1 and A2. Let d1 be the link length of A1. Let ℓi

be the distance from the Ai origin to its center of mass. For each Ai, let mi and

Ii be its mass and moment of inertia, respectively.

The kinetic energy of A1 is

K1(q˙) = 1 m1ℓ1θ˙2 + 1 I1θ˙2, (13.149)

2

and the potential energy of A1 is

1 2 1

The kinetic energy of A2 is

V1(q) = m1gℓ1 sin θ1. (13.150)

K2(q˙) = 1 p · p + 1 I2(θ˙1 + θ˙2)2, (13.151)

2

2

in which p denotes the position of the center of mass of 1 and is given from (3.53) as

A

p1 = d1 cos θ1 + ℓ2 cos θ2 p2 = d1 sin θ1 + ℓ2 sin θ2.

The potential energy of A2 is

(13.152)

V2(q) = m2g(d1 sin θ1 + ℓ2 sin θ2). (13.153) At this point, the Lagrangian function can be formed as

L(q, q˙) = K1(θ˙1) + K2(θ˙1, θ˙2) − V1(θ1) − V2(θ1, θ2) (13.154)

and inserted into (13.118) to obtain the differential constraints in implicit form,

expressed in terms of

q¨,

q˙, and q. Conversion to the phase space is performed

by solving the implicit constraints for q¨ and assigning x = (q, q˙), in which x is a four-dimensional phase vector.

Rather than performing the computations directly using (13.118), the con- straints can be directly determined using (13.140). The terms are

M(q) = m11 m12 , (13.155)

（ ＼

m21 m22

in which

m11 = I1 + m1ℓ2 + I2 + m2(d2 + ℓ2 + 2d1ℓ2 cos θ2)

1 1 2

m12 = m21 = I2 + m2(ℓ2 + d1ℓ2 cos θ2)

2

(13.156)

m22 = I2 + m2ℓ2,

2

and

c111

= 1 ∂m11 = 0

2 ∂θ

c = c

1

= 1 ∂m11 = −m ℓ ℓ p

112 121

2 ∂θ2

2 1 2 2

∂m12 1 ∂m22

c = − = −m ℓ ℓ p

2

122 ∂θ

2 ∂θ1

2 1 2 2

(13.157)

∂m21 1 ∂m11

c = − = m ℓ ℓ p

1

211 ∂θ

2 ∂θ2

2 1 2 2

c212 = c221

1 ∂m22

= = 0

2 ∂θ1

1 ∂m22

c = = 0.

222

The final term is defined as

2 ∂θ2

g1 = (m1ℓ1 + m2d1)gp1 + m1ℓ2p2 g2 = m2ℓ2gp2.

(13.158)

The dynamics can alternatively be expressed using M(q), C(q, q˙), and g(q) in (13.142). The Coriolis matrix is defined using (13.143) to obtain

C(q, q˙) = −m ℓ ℓ p

2

1

2

（θ˙2

θ˙1

θ˙1 + θ˙2＼ , (13.159)

in which p2 is defined in (13.152) and is a function of q. For convenience, let

2

0

r = m2ℓ1ℓ2p2. (13.160)

The resulting expression, which is now a special form of (13.142), is

m11θ¨1 + m12θ¨2 − 2rθ˙1θ˙2 − rθ˙2 + g1(q) = u1

2

m22θ¨1 + m21θ¨2 + rθ˙2 + g2(q) = u2.

1

(13.161)

The phase transition equation is obtained by letting x = (θ1, θ2, θ˙1, θ˙2) and substituting the state variables into (13.161). The variables θ¨1 and θ¨2 become x˙ 3

and x˙ 4, respectively. The equations must be solved for x˙ 3 and x˙ 4. An extension of this model to motors that have gear ratios and nonnegligible mass appears in

[856]. ．

The example provided here barely scratches the surface on the possible systems that can be elegantly modeled. Many robotics texts cover cases in which there are more links, different kinds of joints, and frictional forces [366, 725, 856, 907, 994]. The phase transition equation for chains of bodies could alternatively be de- rived using the Newton-Euler formulation of mechanics. Even though the La- grangian form is more elegant, the Newton-Euler equations, when expressed re- cursively, are far more efficient for simulations of multibody dynamical systems

[366, 863, 994].

• • 1. Extensions of the Euler-Lagrange Equations

Several extensions of the Euler-Lagrange equation can be constructed to handle complications that arise in addition to kinetic energy and potential energy in a conservative field. Each extension usually involves adding more terms to (13.129) to account for the new complication. Problems that can be handled in this way are closed kinematic chains, nonholonomic constraints, and nonconservative forces (such as friction).

• • • 1. Incorporating velocity constraints

The Lagrangian formulation of Section 13.4.1 can be extended to allow additional constraints placed on q and q˙. This is very powerful for developing state transition equations for robots that have closed kinematic chains or wheeled bodies. If there are closed chains, then the configurations may be restricted to lie in a subset of

. If a parameterization of the solution set is possible, then can be redefined over the reduced C-space. This is usually not possible, however, because such a parametrization is difficult to obtain, as mentioned in Section 4.4. If there are wheels or other contact-based constraints, such as those in Section 13.1.3, then extra constraints on q and q˙ exist. Dynamics can be incorporated into the models of Section 13.1 by extending the Euler-Lagrange equation.

C C

The coming method will be based on Lagrange multipliers. Recall from stan- dard calculus that to optimize a function h defined over Rn, subject to an implicit constraint g(x) = 0, it is sufficient to consider only the extrema of

h(x) + λg(x), (13.162)

in which λ R represents a Lagrange multiplier [508]. The extrema are found by solving

∈

∇h(x) + λ∇g(x) = 0, (13.163)

which expresses n equations of the form

∂h

∂xi

∂g

• λ

∂xi

= 0. (13.164)

The same principle applies for handling velocity constraints on C.

Suppose that there are velocity constraints on C as considered in Section 13.1.

Consider implicit constraints, in which there are k equations of the form gi(q, q˙) = 0 for i from 1 to k. Parametric constraints can be handled as a special case of implicit constraints by writing

gi(q, q˙) = q˙i − fi(q, u) = 0. (13.165)

For any constraints that contain actions u, no extra difficulties arise. Each ui is treated as a constant in the following analysis. Therefore, action variables will not be explicitly named in the expressions.

As before, assume time-invariant dynamics (see [789] for the time-varying case).

Starting with L(q, q˙) defined using (13.130), let the new criterion be

k

-

Lc(q, q˙, λ) = L(q, q˙) + λi_g_i(q, q˙). (13.166)

i=1

A functional Φc is defined by substituting Lc for L in (13.114).

The extremals of Φc are given by n equations,

and k equations,

d ∂Lc dt ∂q˙i

d ∂Lc

∂Lc

• ∂qi

∂Lc

= 0, (13.167)

dt ∂λ˙ i

• ∂λi

= 0. (13.168)

The justification for this is the same as for (13.124), except now λ is included. The equations of (13.168) are equivalent to the constraints gi(q, q˙) = 0. The first

term of each is zero because λ˙ does not appear in the constraints, which reduces

them to

∂Lc

∂λi

= 0. (13.169)

This already follows from the constraints on extremals of L and the constraints gi(q, q˙) = 0. In (13.167), there are n equations in n+k unknowns. The k Lagrange multipliers can be eliminated by using the k constraints gi(q, q˙) = 0. This cor- responds to Lagrange multiplier elimination in standard constrained optimization [508].

The expressions in (13.167) and the constraints gi(q, q˙) may be quite compli- cated, which makes the determination of a state transition equation challenging. General forms are given in Section 3.8 of [789]. An important special case will be considered here. Suppose that the constraints are Pfaffian,

n

-

gi(q, q˙) = gij (q)q˙j = 0, (13.170)

j=1

as introduced in Section 13.1. This includes the nonholonomic velocity constraints due to wheeled vehicles, which were presented in Section 13.1.2. Furthermore, this includes the special case of constraints of the form gi(q) = 0, which models closed kinematic chains. Such constraints can be differentiated with respect to time to obtain

d n ∂g n

g (q) = - i q˙ = - g

dt

i

∂q

j

ij

j

j=1

j

j=1

(q)q˙

= 0, (13.171)

j=1

j

j=1

which is in the Pfaffian form. This enables the dynamics of closed chains, con- sidered in Section 4.4, to be expressed without even having a parametrization of

the subset of that satisfies the closure constraints. Starting in implicit form, differentiation is required to convert them into the Pfaffian form.

C

For the important case of Pfaffian constraints, (13.167) simplifies to

d ∂L dt ∂q˙i

∂L

• ∂qi

k

• λj gji(q) = 0, (13.172)

-

j=1

The Pfaffian constraints can be used to eliminate the Lagrange multipliers, if desired. Note that gji represents the ith term of the jth Pfaffian constraint. An action variable ui can be placed on the right side of each constraint, if desired.

Equation (13.172) often appears instead as

d ∂L dt ∂q˙i

∂L

• ∂qi

k

= λj gji(q, q˙), (13.173)

-

l=1

which is an alternative but equivalent expression of constraints because the La- grange multipliers can be negated without affecting the existence of extremals. In this case, a nice interpretation due to D’Alembert can be given. Expressions that appear on the right of (13.173) can be considered as actions, as mentioned in Section 13.4.1. As stated previously, such actions are called generalized forces in mechanics. The principle of virtual work is obtained by integrating the reaction forces needed to maintain the constraints. These reaction forces are precisely given on the right side of (13.173). Due to the cancellation of forces, no true work is done by the constraints (if there is no friction).

Example 13.14 (A Particle on a Sphere) Suppose that a particle travels on a unit sphere without friction or gravity. Let (q1, q2, q3) R3 denote the position of the point. The Lagrangian function is the kinetic energy,

∈

L(q, q˙) = 1 m(q˙2 + q˙2 + q˙2), (13.174)

2 1 2 3

in which m is the particle mass. For simplicity, assume that m = 2.

The constraint that the particle must travel on a sphere yields

g1(q) = q2 + q2 + q2 − 1 = 0. (13.175)

1

2

3

This can be put into Pfaffian form by time differentiation to obtain

2q1q˙1 + 2q2q˙2 + 2q3q˙3 = 0. (13.176)

Since k = 1, there is a single Lagrange multiplier λ1. Applying (13.172) yields three equations,

q¨i − 2qiλ1 = 0, (13.177)

for i from 1 to 3. The generic form of the solution is

c1q1 + c2q2 + c3q3 = 0, (13.178)

in which the ci are real-valued constants that can be determined from the initial position of the particle. This represents the equation of a plane through the origin. The intersection of the plane with the sphere is a great circle. This implies that the particle moves between two points by traveling along the great circle. These

are the shortest paths (geodesics) on the sphere. ．

The general forms in Section 13.4.2 can be extended to the constrained case.

For example, (13.142) generalizes to

M(q)q¨ + C(q, q˙)q˙ + g(q) + G(q)T λ = u, (13.179)

in which G is a n k matrix that represents all of the gji Pfaffian coefficients. In this case, the Lagrange multipliers can be computed as [725]

×

λ = G(q)M(q)−1G(q)T −1 G(q)M(q)−1 u − C(q, q˙)q˙ , (13.180)

（ ） （ ）

assuming G is time-invariant.

The phase transition equation can be determined in the usual way by perform- ing the required differentiations, defining the 2n phase variables, and solving for x˙ . The result generalizes (13.148).

• • • 1. Nonconservative forces

The Lagrangian formulation has been extended so far to handle constraints on that lower the dimension of the tangent space. The formulation can also be extended to allow nonconservative forces. The most common and important ex- ample in mechanical systems is friction. The details of friction models will not be covered here; see [681]. As examples, friction can arise when bodies come into con- tact, as in the joints of a robot manipulator, and as bodies move through a fluid, such as air or water. The nonconservative forces can be expressed as additional generalized forces, expressed in an n 1 vector of the form B(q, q˙). Suppose that an action vector is also permitted. The modified Euler-Lagrange equation then

C

×

becomes

d ∂L

dt ∂q˙ −

∂L

= u B(q˙, q). (13.181)

−

∂q

A common extension to (13.142) is

M(q)q¨ + C(q, q˙)q˙ + N(q, q˙) = u, (13.182)

in which N(q, q˙) generalizes g(q) to include nonconservative forces. This can be generalized even further to include Pfaffian constraints and Lagrange multipliers,

M(q)q¨ + C(q, q˙)q˙ + N(q, q˙) + G(q)T λ = u. (13.183) The Lagrange multipliers become [725]

（ ） （ ）

λ = G(q)M(q)−1G(q)T −1 G(q)M(q)−1 u − C(q, q˙)q˙ − N(q, q˙) . (13.184)

Once again, the phase transition equation can be derived in terms of 2n phase variables and generalizes (13.148).

• • 1. Hamiltonian Mechanics

The Lagrangian formulation of mechanics is the most convenient for determin- ing a state transition equation for a collection of bodies. Once the kinetic and potential energies are determined, the remaining efforts are straightforward com- putation of derivatives and algebraic manipulation. Hamiltonian mechanics pro- vides an alternative formulation that is closely related to the Lagrangian. Instead of expressing second-order differential constraints on an n-dimensional C-space, it expresses first-order constraints on a 2n-dimensional phase space. This idea should be familiar from Section 13.2. The new phase space considered here is an example of a symplectic manifold, which has many important properties, such as being orientable and having an even number of dimensions [39]. The standard phase vector is defined as x = (q, q˙); however, instead of q˙, n variables will be introduced and denoted as p. Thus, a transformation exists between (q, q˙) and (p, q). The p variables are related to the configuration variables through a special function over the phase space called the Hamiltonian. Although the Hamiltonian formulation usually does not help in the determination of x˙ = f(x, u), it is covered here because its generalization to optimal control problems is quite powerful. This generalization is called Pontryagin’s minimum principle and is covered in Section

15.2.3. In the context of mechanics, it provides a general expression of energy conservation laws, which aids in proving many theoretical results [39, 397].

The relationship between (q, q˙) and (p, q) can be obtained by using the Legendre transformation [39, 397]. Consider a real-valued function f of two variables, x, y

∈

R. Its total differential [508] is

df = u dx + v dy, (13.185)

in which

∂f

u = and v =

∂x

∂f

. (13.186)

∂y

Consider constructing a total differential that depends on du and dy, instead of

dx and dy. Let g be a function of u and y defined as

g(u, y) = ux − f. (13.187)

The total differential of g is

dg = x du + u dx − df. (13.188)

Using (13.185) to express df , this simplifies to

dg = x du − v dy. (13.189) The x and v variables are now interpreted as

∂g ∂g

x = ∂u v = −∂y , (13.190)

which appear to be a kind of inversion of (13.186). This idea will be extended to vector form to arrive the Hamiltonian formulation.

Assume that the dynamics do not depend on the particular time (the exten- sion to time-varying dynamics is not difficult; see [39, 397]). Let L(q, q˙) be the Lagrangian function defined (13.129). Let p Rn represent a generalized momen- tum vector (or adjoint variables), which serves the same purpose as u in (13.185). Each pi is defined as

∈

∂L

pi = ∂q˙ . (13.191)

i

In some literature, p is instead denoted as λ because it can also be interpreted as a vector of Lagrange multipliers. The Hamiltonian function is defined as

n

H(p, q) = p · q˙ − L(q, q˙) = pi_q˙_i − L(q, q˙) (13.192)

-

i=1

and can be interpreted as the total energy of a conservative system [397]. This is a vector-based extension of (13.187) in which L and H replace f and g, respectively. Also, p and q are the vector versions of u and x, respectively.

Considered as a function of p and q only, the total differential of H is

n ∂H n ∂H

-

-

dH = ∂p

i

i=1

dpi + ∂q

i=1

i

dqi. (13.193)

Using (13.192), dH can be expressed as

n n n ∂L

n ∂L

dH = - q˙i dpi + - pi dq˙i − - ∂q˙ dq˙i − - ∂q dqi. (13.194)

i=1

i=1

i=1

i

i=1

i

The dq˙i terms all cancel by using (13.191), to obtain

n n ∂L

dH = - q˙i dpi − - ∂q dqi. (13.195)

i=1

i=1

i

Using (13.118),

This implies that

p˙ =

n

∂L

∂qi

. (13.196)

n

dH = - q˙i dpi − - p˙i dqi. (13.197)

i=1

i=1

i=1

i=1

Equating (13.197) and (13.193) yields 2n equations called Hamilton’s equations:

q˙i =

∂H

∂pi

p˙i =

∂H

∂qi

, (13.198)

for each i from 1 to n. These equations are analogous to (13.190).

Hamilton’s equations are equivalent to the Euler-Lagrange equation. Extremals in both cases yield equivalent differential constraints. The difference is that the Lagrangian formulation uses (q, q˙) and the Hamiltonian uses (p, q). The Hamilto- nian results in first-order partial differential equations. It was assumed here that the dynamics are time-invariant and the motions occur in a conservative field. In this case, dH = 0, which corresponds to conservation of total energy. In the time-varying case, the additional equation ∂H/∂t = ∂L/∂t appears along with Hamilton’s equations. As stated previously, Hamilton’s equations are primarily of interest in establishing basic results in theoretical mechanics, as opposed to determining the motions of particular systems. For example, the Hamiltonian is used to establish Louisville’s theorem, which states that phase flows preserve vol- ume, implying that a Hamiltonian system cannot be asymptotically stable [39]. Asymptotic stability is covered in Section 15.1.1. Pontryagin’s minimum princi- ple, an extension of Hamilton’s equations to optimal control theory, is covered in 15.2.3.

−