Dartmouth CS 54: Dynamics

Dynamics

Dynamic equations for point systems: forces, momenta

We'll start with a single particle moving along a trajectory in the plane. The position of the particle is $x = (x_1(t), x_2(t))$ . The velocity vector is $\dot x = (\dot x_1, \dot x_2)$ .

At any given time, the momentum $p$ of the particle is defined as

(1)

$p = m \dot x$

The momentum vector is constant unless forces are acting on the particle; the rate of change of momentum is the force:

(2)

$f= \frac{d}{dt} p = \frac{d}{dt} m \dot x = m \ddot x$

Imagine that there is a force field over the plane, $f(x_1, x_2)$ . We have

(3)

$\ddot x = f(x)/m$

Given an initial position and velocity for the particle $( x(0), \dot x(0) )$ we could compute the initial acceleration, and integrate the acceleration twice to find the velocity and position of the particle at each time.

Numerical integration of dynamics equations

If the force field is constant (for example, an approximation of a gravitational field near the earth's surface), we might be able to find an analytical solution, but otherwise numerical integration is probably needed.

Euler integration

The simplest choice (but probably not the best choice) is to use Euler integration. For some differential equation

(4)

$\dot x = f(x)$

choose some time step $\Delta t$ . Then use a finite difference equation to compute an approximation of $x$ at the next time step:

(5)

$x(t + \Delta t) = x(t) + \Delta t \cdot f(x).$

There is a bit of a problem: we have a second-order differential equation! Folding the constant $m$ into the function f,

(6)

$\ddot x = f(x).$

We could solve for the velocity at time step 1, and use the velocity from time step 0 to approximate the position at time step 1. Then use the velocity and position from time step 1 to compute the velocity and position at time step 2, etc.

There is a nice trick that lets you write any n-th order differential equation (with n dots over the x) as a first-order differential equation.

Define $y = (x, \dot x)$ . The four-dimensional vector $y$ is called the state of the system, and is composed of the configuration and the velocity of the system. Notice that we have

(7)

$\dot y = \left(\begin{matrix} \dot x \ f(x)\\ \end{matrix} \right) = \left(\begin{matrix} y_3 \ y_4 \ f_1(y_1, y_2)\ f_2(y_1, y_2) \end{matrix} \right)$

That is, $\dot y = g(y)$ , a first-order differential equation. For this reason, numerical integration methods are typically written to solve only first-order differential equations.

Smoother integration techniques: Midpoint, Runge-Kutta

Numerical integration techniques like the Euler-step method rely on the assumption that the force applied to the system (or more generally, the function of state) does not change too rapidly with time.

The Euler method computes the velocity at the beginning of the time step, and assumes this velocity is constant over the entire time step. This is a problem particularly if the velocity is always changing in the same direction. For example, consider the differential equation

(8)

$\dot x = (-x_2, x_1)$

with $x(0) = (1, 0)$ . We know this to be the equation of a circle. We will get an outward spiral if we apply the Euler step method, though.

One idea is to compute an estimate of the configuration halfway through the time step (using an Euler approximation), use that configuration to compute the velocity vector, and then use that velocity vector (and the original configuration) to estimate the configuration at the end of the time step. This is called the midpoint method:

(9)

$x_{n + 1} = x_{n} + \Delta t f( x_n + \Delta_t /2 f(x_n) )$

There is a worst-case analysis of Euler and midpoint methods on Wikipedia. It turns out the the expected error of the Euler-step method is $O(\Delta t^2)$ and only $O(\Delta t^3)$ for the midpoint method. Since twice the computation is required for the midpoint method, it would seem that the expected error per unit of computation is similar.

However, notice that the effect of using midpoint instead of Euler is not the same as simply halving the size of the time step. For the circle example, we expect the midpoint method to stay much closer to the actual trajectory, rather than spiralling out as the Euler method does. The "worst-case" curves for the two methods are different.

The idea of the midpoint method is to compute the vector f at a "better" place than at the start of the time step. This idea is taken further by popular Runge-Kutta methods. In RK4, four vectors are computed, and the weighted average is used to compute the result of the time step. For smooth systems, these methods are expected to give much better convergence than midpoint or Euler, per unit of computation.

There are many other numerical integration techniques, designed to solve various classes of differential equations. For example, if the forces on the system are large, we say that the system is stiff. (Imagine a stiff spring, or a piece of cloth with very high forces exerted to keep the cloth from stretching). For these systems, methods like those we have described tend to be unstable — error in position computation leads to large restoring forces, leading to even larger errors on the next time step. Implicit integration techniques tend to be more stable, allowing larger time steps.

Energy and work

In high-school physics, there was a definition of the work done by a particle as the force applied to the particle multiplied by the distance the particle moved. However, we only "count" the amount of force that is parallel to the direction of motion. For example, a particle moving horizontally in a vertical gravitational force field does no work versus gravity.

If the particle is moving along a trajectory, we have the following definition of work:

(10)

$W = \int_0^T f(x) \cdot \dot x dt$

Mathworld gives the following definition of a conservative system:

the work done by a force in a conservative system is

Independent of path.
Equal to the difference between the final and initial values of an energy function.
Completely reversible.

Gravitational and electric fields are conservative, and can be written in terms of potential engergy functions, such that the negative gradient of the function gives the direction and manitude of the force at any point. For example, define $V_g = mgy$ to be the potential energy function associated with a gravitaional field. Computing the negative gradient vector $-(\partial V / \partial x, \partial V / \partial y)$ gives
$(0, -mg)$ , a downwards gravitational force.

Navigation functions; artificial potential fields

Let's step away from the dynamics problem and consider a robotics application that this formulation inspires: navigation of a point robot among obstacles to a goal. It really has nothing to do with dynamics.

Taking the gradient of a smooth energy function at a point gives a force in a direction. You could imagine an energy function over the configuration space so that obstacles were high energy, free space were lower energy, and the goal was zero energy. The robot simple "rolls downhill" (follows the gradient of the potential function) until it reaches the goal.

Artificial potential fields of this type are a very popular method for obstacle avoidance, in part because of how easy they are to implement. For example, define $d_g$ to be the Euclidean distance of the robot from the goal, and $d_i$ to be the distance from the i-th obstacle. Let

(11)

$V = k_g d_g - k_1 d_1 - k_2 d_2 - k_3 d_3 \ldots$

where the k's are positive constants that force the robot towards the goal or away from obstacles.

V has a unique global minimum at the goal, so we might hope that the robot will reach that minimum by following the gradient. There is a problem, however — the robot may be caught in a local minimum. There are a few solutions to this that people have tried:

create an escape strategy to get out of local minima
ensure that there are no local minima

A simple escape stragegy is to cause the robot to thrash randomly when it detects that it is stuck — this is sometimes called "Brownian motion" escape strategy.

There are different ways to ensure that there is only a global minimum. One method is to cause the potential energy function to be the time cost of the fastest path to the goal from each point in the configuration space. Of course, if we could compute that, we wouldn't need a potential energy function! (Notice how comparitively easy the quadratic potential function above is.) We have seen an example of this: the wavefront dynamic programming planner on a grid.

Another approach is to place restrictions on the shapes of the obstacles, and prove that with those restrictions certain simple-to-compute potential functions do not have local minima. This is the approach taken by Koditschek's work on navigation functions.

Kinetic energy

Ok, back to the phsyics. Define the kinetic energy T to be

(12)

$T = \frac{1}{2}\sum_i m_i v_i^2$

where $m_i, v_i$ are the mass and velocity of the i-th particle. In a conservative system, the sum H = T + V, called the Hamiltonian, is conserved (constant). Forces that do work (have a component that is not orthogonal to the velocity of the system) can add or remove energy from a non-conservative system.

The kinetic and potential energies of the system can be used to derive other quantities of interest, including forces and momenta. This is particularly interesting because the energies are scalar quantities and may often be computed in a straightforward way even for complicated systems.

But let's consider first a simple particle in a gravitational field. We have $T= \frac{1}{2}mv^2$ and $V = mgh$ .

First, notice that the momentum of the particle can be computed by taking the partial derivatives of the kinetic energy with respect to the coordinate velocities $\dot x$ and [[\dot y]]:

(13)

$T = \frac{1}{2} m (\dot x^2 + \dot y^2)$

(14)

$p = \left(\begin{matrix} \partial T / \partial \dot x \ \partial T / \partial \dot y \end{matrix} \right) = \left(\begin{matrix} m \dot x \ m \dot y \end{matrix} \right)$

The force on the particle is the rate of change of the momentum:

(15)

$f = \frac{d}{dt} \left(\begin{matrix} \partial T / \partial \dot x \ \partial T / \partial \dot y \end{matrix} \right) = \left(\begin{matrix} m \dot x \ m \dot y \end{matrix} \right)$

The force can be computed as a gradient of the potential energy:

(16)

$f = - \left(\begin{matrix} \partial V / \partial \dot x \ \partial V / \partial \dot y \end{matrix} \right)$

So for a particle, we have the equations of motion written in terms of energy.

(17)

$\frac{d}{dt} \frac{\partial}{\partial \dot x} T = - \frac{\partial} {\partial x} V$

(18)

$\frac{d}{dt} \frac{\partial}{\partial \dot y} T = - \frac{\partial} {\partial y} V$

That is, the rate of change of momentum (which is the partials of the kinetic energy with respect to the velocity) is a vector along the negative gradient of the potential energy field.

Define the term $L = T - V$ , called the Lagrangian of the system. Since $T$ is independent of the position, and $V$ is independent of the velocity, we could write the previous equations as:

(19)

$\frac{d}{dt} \frac{\partial}{\partial \dot x_i} L - \frac{\partial} {\partial x_i} L = 0$

where $x_i$ is the i-th coordinate. What happens if there are external forces? We just put them on the right hand side:

(20)

$\frac{d}{dt} \frac{\partial}{\partial \dot x_i} L - \frac{\partial} {\partial x_i} L = f_{\mathrm {ext}}$

We now have a description of the equations of motion in terms of a single scalar quantity, the Lagrangian, and the external forces. Why should we do such a thing? We will see in the next section that this equation holds even if we write the equations in generalized coordinates. This means we can move from writing three equations per particle to writing a single equation for each freedom (or generalized coordinate) of the system, and do not have to express constraint forces explicitly.

Lagrangian dynamics

For a dynamic mechanical system, we will see that the equations of motion are:

(21)

$\frac{d}{dt} \frac{\partial}{\partial \dot q_i} L - \frac{\partial} {\partial q_i} L = J^T F_{ext}$

where $F$ are the external forces applied to particles of the system.

Simple pendulum example

Let's take a simple example of a pendulum with no external forces; all forces are caused by the conservative gravitational potential field. Let the configuration of the pendulum be given by $q = \theta$ . We then write expressions for kinetic and potential energy in terms of q:

(22)

$T = \frac{1}{2} m v^2 = \frac{1}{2} m r^2 \dot \theta ^2$

(23)

$V = mgh = m g r \sin \theta$

The Lagrangian is

(24)

$L = T - V = \frac{1}{2} m r^2 \dot \theta ^2 - mg r \sin \theta$

We compute the first partial term:

(25)

$\frac{\partial}{\partial \dot \theta} L = m r^2 \dot \theta$

and the time derivative:

(26)

$\frac{d}{dt} \frac{\partial}{\partial \dot \theta} L = m r^2 \ddot \theta$

The partial of L with respect to $\theta$ :

(27)

$\frac{\partial}{\partial \theta} L = - mgr \cos \theta$

So the equations of motion are:

(28)

$m r^2 \ddot \theta + mgr \cos \theta = 0$

We can simplify and solve for $\ddot \theta$ :

(29)

$r \ddot \theta + g \cos \theta = 0$

(30)

$\ddot \theta = - \frac{g \cos \theta}{r}$

We could numerically integrate to simulate the system, by taking the state to be $(\theta, \dot \theta)$ and using e.g. RK 4 on the resulting first-order differential equation. Or we solve analytically (not possible for many systems, but this one is simple) to find the trajectories for a simple harmonic oscillator. Or we could analyze what happens to the system if forces are applied by motors or a finger.

Derivation of Lagrangian dynamics equations

There are many ways to derive the Lagrangian formulation, but most of the derivations make use of a somewhat abstract idea called "virtual work" (for example, the Wikipedia article uses this approach). We will return to the idea of virtual work, but for now, I'll give a proof of the Lagrangian equations that is based only on $f = ma$ for particles and the differential kinematics you are already familiar with. The proof is based roughly on Symon's "Mechanics", page 309, published in 1953, but I will use our own vector and matrix notation.

Remember that for particles, the momentum of the system is given by the partial derivatives of the kinetic energy with respect to velocity. The forces are given by the time derivative of that. Therefore, we will start by computing the quantity

(31)

$\frac{d}{dt} \frac{\partial T}{\partial \dot q_k}.$

Let $x = (x_1, y_1, z_1, x_2, y_2, z_2, \ldots z_n)$ , let $\hat M$ be the diagonal matrix with the first three elements $m_1$ , next three $[[m_2]]$, etc. Let $J$ be the Jacobian such that $\dot x = J \dot q$ .

First, we need an expression for the kinetic energy of the system.

(32)

$T = \frac{1}{2} \dot x^T \hat M \dot x$

Side note: an expression of the form "transpose of a vector times symmetric positive definite matrix times the same vector" is called a quadratic form. Quadratic forms take a vector as input and return a scalar, and the scalar is positive if the vector has non-zero length.

We can also write the kinetic energy as a function of the generalized velocities. Notice that $\dot x ^ T = \dot q^T J^T$ , so:

(33)

$T = \frac{1}{2} \dot q^T J^T \hat M J \dot q$

This is also a quadratic form, but with the input variable $\dot q$ and a different matrix. The matrix $M = J^T \hat M J$ has a special name; it is called the mass matrix, and we will see it again and again in formulations of dynamics problems. Notice that the size of the square mass matrix is equal to the number of generalized coordinates.

(34)

$T = \frac{1}{2} \dot q^T M \dot q$

The generalized momentum vector $p$ is the vector of partials of the kinetic energy with respect to each generalized velocity:

(35)

$p = \frac{\partial T}{\partial \dot q} = \frac{\partial}{\partial \dot q} \frac{1}{2} \dot q^T M \dot q$

It turns out that the vector of partials of a quadratic form with respect to the input vector can be computed simply, if the matrix is independent of the input vector. That is, for any vector $y$ , and any matrix Q that is not a function of y, $\frac{\partial}{\partial y} y^T Q y = 2 Q y$ . To prove this trick works, just write out all the elements of the vector and matrix and take the partials.

(36)

$p = \frac{\partial}{\partial \dot q} \frac{1}{2} \dot q^T M \dot q = M \dot q$

This momentum is entirely analogous to the one-dimensional case: momentum is the product of mass and some velocity — in this case, our mass matrix is a bit more complicated because of the "change of coordinates" by the Jacobian.

Now we want to look at the rate of change of the momenta. Let's go back to talking about the motion of particles rather than generalized coordinates. We have:

(37)

$p = M \dot q = J^T \hat M \dot x$

If we take the time derivative, we get:

(38)

$\frac{d}{dt} p = J^T \hat M \ddot x + \frac{d}{dt} (J^T \hat M) \dot x = J^T \hat M \ddot x + {\dot J}^T \hat M \dot x$

From Newton's laws, if the forces applied to the particles are $F$ , then $F = \hat M \ddot x$ , so we have

(39)

$\frac{d}{dt} p = J^T F + {\dot J}^T \hat M \dot x$

The term $J^T F$ can be thought of as the effect that forces applied at the particles has on the "joints". In fact, these terms have a special name, the generalized forces, and sometimes are given the symbol $Q$ . A torque is an example of generalized force; the torque is actually caused by forces applied at points, but the net effect is an angular acceleration $\ddot \theta$ .

But there's this other term: ${\dot J}^T \hat M \dot x$ . I don't have a good proof in vector notation, but if you write out all the terms in the matrix multiplication, you can show that ${\dot J}^T \hat M \dot x = \frac{\partial}{\partial q}T$ .

We therefore have:

(40)

$\frac{d}{dt} \frac{\partial}{\partial \dot q} T - \frac{\partial}{\partial q}T= Q$

where the generalized force $Q$ has components due to external forces and due to the gradient of the potential energy.

(41)

$Q = J^T F_{ext} -\frac{\partial V}{\partial q}$

This is a slightly alternate form of the Lagrangian equations in the last section.

Standard form for dynamics equations

Remember that the generalized momentum can be written as

(42)

$p = M \dot q$

The dynamics equations are

(43)

$\frac{d}{dt} \frac{\partial}{\partial \dot q} T - \frac{\partial}{\partial q}T = Q$

(44)

$\frac{d}{dt} M \dot q + \frac{\partial}{\partial q} \dot q^T M \dot q = Q$

(45)

$M \ddot q + \dot M \dot q + \frac{\partial}{\partial q} (\dot q^T M \dot q) = Q$

Without going into the details, the term $\frac{\partial}{\partial q}$ turns out to be a matrix mutliplied by $\dot q$ . The form of the equations is then

(46)

$M(q) \ddot q + C(q, \dot q) \dot q = Q$

Rigid bodies

The generalized coordinates for a rigid body are usually chosen to be the location of some reference point on the body, as well as additional coordinates to express the rotation of the body. It therefore turns out that the generalized forces have a translational component (the net force on the body) and a component that affects angular acceleration (the net torque on the body). The mass matrix and kinetic energy also have a special structure:

(47)

$T = \frac{1}{2} (m v^2 + \omega^T L \omega)$

where $\omega$ is the 3-vector giving the rotational velocity, and $L$ is a matrix called the inertia tensor. The inertia tensor depends on the configuration of the body, but is constant when viewed in the body frame. Paritosh found this excellent set of notes describing further properties of the inertia tensor: The Inertia Tensor and After Dinner Tricks, by Steve Drasco.

page_revision: 44, last_edited: 1236002160|%e %b %Y, %H:%M %Z (%O ago)

Edit Tags History Files Print Site tools + Options

Dartmouth CS 54

Introduction to Robotics

Course

Links

Wiki