We’ll start with a review of vectors, matrices, and derivatives. Then we’ll move to discussion of velocities in configuration space (rates of change of joint angles), and how they relate to velocities of particular points on the robot.
Wikipedia has a useful page on vectors. A vector is a direction in space, with an associated magnitude. Vectors have a initial point and a terminal point, but often we do not write down the initial point, and instead just write the vector as the difference between the two points. For example, we would say that the vector from the initial point (2, 3, 1) to the terminal point (10, 10, 10) is (8, 7, 9). We should not forget that the vector has an initial point.
Vectors can be written vertically (a column vector) or horizontally (a row vector). Typically we will write vectors as column vectors, i.e.
\(x = \left(\begin{matrix} 8 \\ 7 \\ 9 \end{matrix} \right)\)
Transposition changes a row vector to a column vector, and vice versa. Transposition is denoted by a superscript T; \(x^T = (8, 7, 9)\).
We say that a set of vectors is independent if none of the vectors can be expressed as a linear combination of the others.
We will use vectors in many ways; one use is to express position. We label some point in the world the origin, and any other location can be expressed by a vector giving the location of that location with respect to the origin.
The dot product of two vectors returns a scalar, and can be computed in two ways. First, you can multiply the corresponding elements of the vectors and add them: \(a \cdot b = a_1 b_1 + a_2 b_2 + a_3 b_3 +\ldots\). The length of a vector is the square root of the vector of the dot product of that vector with itself. \(||a|| = \sqrt{a \cdot a}\).
The second way to compute a dot product is as the cosine of the angle between the two vectors, multiplied by the lengths of the vectors. \(a \cdot b = ||a|| ||b|| \cos \alpha\). The equivalence of these two formulas define cosine in higher dimensions. If you want to compute the angle between two vectors, scale the vectors be be unit vectors, and take the arccosine of the dot product.
Dot products are discussed at more length on Wikipedia. You should make sure you understand the geometric interpretation of dot products as a scaled projection of one vector onto another.
There are several different types of notation for the dot product. Sometimes you’ll see a dot between the vectors: \(a \cdot b\). Sometimes you’ll simply see a row vector multiplied by a column vector: \(a^T b\). Sometimes you’ll see angle brackets: \(\langle a, b \rangle\).
A frame in three dimensions is a collection of three independent vectors with the same initial point. Typically we will consider orthogonal frames, where the dot product of any pair of vectors is zero. We will also use only vectors of unit length.
We can attach a frame to the world, to a rigid body, or to any other portion of the world. For example, to describe a coordinate system on the world, we chose some point and call it the origin, then attach a frame to that point so that the vectors point in convenient directions, which we might call x, y, and z.
We can describe the orientation of a frame using a matrix. For example,
\(F_1 = \left[ \begin{array}{ccc} \sqrt{3}/2 & -.5 & 0\\ .5 & \sqrt{3}/2 & 0\\ 0 & 0 & 1 \end{array} \right]\)
The columns of the matrix are the three vectors that make up the frame. We would also need to know where the origin of the frame was. Let’s say that the origin of the frame \(F_1\) is (0, 2, 0).
We can attach a frame rigidly to a rigid body – this is called a
body-fixed frame. Locations of points on the body can then be expressed
relative to a frame. For example, we might talk about a point \(^1p_1\) with location \((1, 0, 0)\) in the frame \(F_1\). The superscript before the variable
indicates that the vector is expressed relative to that frame. The
coordinates of that same point would be very different if expressed
relative to the world frame: in this case, \((\sqrt{3}/2, .5, 0)\).
Sometimes we leave the superscript off, if the position is relative to a
world frame, or we have been very careful to say which frame we intend
the position to be relative to.
If we attach a frame to a rigid body, the location of the frame, as well as the matrix associated with the frame, determine where every point in the rigid body is. Therefore, when we think about moving rigid bodies, we tend to attach a frame to them, and then perform all computations on that frame, rather than worrying about where the individual particles in the body are. This is one reason rigid-body models are so convenient.
We’ll be using frames as coordinate systems, and there is one more convention to know about: handedness. If you point the fingers of your right hand along the first vector, and curl your fingers towards the second vector, your thumb will point either directly along the third vector of an orthogonal frame, or directly oppose the third vector. We say the frame, or coordinate system, is “right-handed” if your thumb points along the third vector. Unless otherwise specified, we will use right-handed frames.
If you’ve taken linear algebra or a basic physics class, you’ve seen the cross product. The cross-product is an operation that takes two vectors in \(\mathbf R^3\) and returns a third vector. The direction of the new vector is perpendicular to each of the first two vectors, and the magnitude of the new vector is equal to the scalar product of the magnitudes of the first two vectors, times the sine of the angle between those vectors. But there are two directions perpendicular to the first two vectors. The cross-product is defined so that if you wrap the finger of your right hand from the first vector to the second, your thumb points in the direction of the computed vector.
Notice that the handedness of cross products means that \(a \times b = - b \times a\); a minus sign is introduces if you swap the orders of the vectors. Formally, this property is called “skew-symmetry”.
Given the first two vectors of a right-handed orthogonal frame, you can construct the third vector by taking the cross product. Maybe you don’t remember how to compute the cross product. One option is to look up the formula on Wikipedia. But there’s also a clever trick to remember how. In physics the coordinate vectors are sometimes called i, j, and k. So the sum \(3\mathbf{i} + 2\mathbf{j} + \mathbf{k}\) is an alternate way of writing the vector \((3, 2, 1)\). It turns out that the cross product of two vectors can be written as the determinant of a matrix in which i, j, and k are the elements of the first row. (It may seem odd to have coordinate directions be elements of a matrix, and to compute a determinant that is not a scalar. This is called an “abuse of notation”.)
\(a \times b = \mathrm{Det} \left[ \begin{array}{ccc} \mathbf{i}& \mathbf{j} & \mathbf{k} \\ a_1 & a_2 & a_3\\ b_1 & b_2 & b_3 \end{array} \right]\)
Of course, you might not remember how to compute the determinant of a 3x3 matrix. Fortunately, Wikipedia does.
You remember how to multiply matrices by vectors and matrices by matrices. There are useful geometric interpretations of matrix multiplication. A matrix is a rectangle containing numbers.
We could also think of a matrix as being a row vector whose elements are column vectors. That is,
$Ax = x = a_1 x_1 + a_2 x_2 + a_3 x_3 $
where \(a_1\), \(a_2\), and \(a_3\) are the columns of \(A\). The matrix-vector product scales each of these vectors by an element of \(x\) and takes the sum. So, matrix multiplication can be seen as taking a weighted sum of the column vectors of the matrix.
A matrix could also be viewed as a column vector whose elements are row vectors. If \(b_1\), \(b_2\), and \(b_3\) are the rows of the matrix B, then
\(B x = \left( \begin{array}{c} b_1 \cdot x \\ b_2 \cdot x \\ b_3 \cdot x \end{array} \right)\)
That is, the elements of the resulting vector are the projection of the original vector onto each of the rows of the matrix. We’ll come back to this idea in the future when we study robot grasping.
Imagine you have a trajectory for a particle in the space \(R^3\), \(x(t), y(t), z(t)\). Each of the coordinates is expressed as a function of time. You could write three functions of time, or you could collect the functions into a vector, creating a vector-valued function.
Now we might ask what the velocity of the particle is at some particular time. You could compute this velocity by taking the time derivatives of each of the functions, giving \(dx/dt\), \(dy/dt\), and \(dz/dt\). Usually, we will use a dot to represent derivatives with respect to time: \(\dot x\), \(\dot y\), and \(\dot z\).
Sometimes, we have a function of several variables, and we want to see how that function changes as we move in the direction of one of the variables. For example, maybe we have a function \(f(x, y)\) that gives the temperature at some location in the \((x, y)\) plane, and we’d like to know the rate of change of the temperature as we move in the increasing \(y\) direction. We’d write this as \(\frac{d f}{d y}\). We’d compute this partial derivative of \(f\) by holding \(x\) constant and differentiating \(f\) with respect to \(y\).
Here’s another example. Consider a 2R planar robot arm. The \(x\) coordinate of the end effector is a function of the two \(\theta\) values:
\(x(\theta_1, \theta_2) = l_1 \cos(\theta_1) + l_2 \cos(\theta_1 + \theta_2)\)
How does \(x\) change if we just change the first angle, \(\theta_1\)?
\(\frac{d x}{d \theta_1} = l_1 \sin(\theta_1) + l_2 \sin (\theta_1 + \theta_2)\)
How about as we change \(\theta_2\)?
\(\frac{d x}{d \theta_2} = l_2 \sin (\theta_1 + \theta_2)\)
We will look at the relationship between velocities at joints and velocities of points attached to the robot.
Assume the configuration space is parameterized by some vector \(q\). For example, for a planar rigid body, we might have \(q = (x, y, \theta)\), and for a 3R planar arm we might have \(q = (\theta_1, \theta_2, \theta_3)\).
At each instant in time, \(t\), the system has some configuration \(q(t)\). For the planar rigid body, we have \(q(t) = (x(t), y(t), \theta(t))\). The configuration is thus a ‘’vector-valued function of time’‘. The trajectory describes a curve in the configuration space, and we say that the curve is’‘parameterized by time’’. We can start the clock whenever we like, so often we will consider a trajectory with time in some interval, \(t \in [0, T]\), such that the start time of the trajectory is zero and the end time is T.
We typically expect the trajectory to be a continuous function of time, except for “rollovers” of angles from \(0\) to \(2\pi\) or vice versa. (In fact, the trajectories ‘’are’’ continuous even at these points, if we view them correctly in the configuration space. Remember, for example, that the topology of the configuration space for a 2R arm is a torus.)
A formal mathematical framework allows us to leverage existing techniques and theorems from the mathematics community to solve robotics problems. We will use some terminology from differential geometry.
First, there is the idea of a tangent to a parameterized curve at at time \(t\):
\(\frac {d{x}}{dt} =\lim_{h \rightarrow 0} \frac{1}{h}({x}(t + h) - {x}(t)),\)
where \(x\) is the vector of coordinates describing a configuration. The tangent has a physical interpretation: it describes the velocity of the elements of the system.
There are problems with how you define the tangent to the trajectory at the boundary between \(0\) and \(2\pi\) angles, but these problems disappear if we remember that the system is defined as a collection of particles, and the configuration gives the location of every particle. Each particle, if following a smooth trajectory, has a well-defined tangent vector, even if our parameterization of the configuration space has some inconvenient sharp edges.
Since every trajectory is embedded in the configuration space surface, the constraints on the motion of the system constrain the possible tangents to a trajectory at a point. We define the tangent space: The tangent space at a point on a surface is set of all possible tangents through that point. We denote the tangent space at a point x with the notation \(T^*({x})\).
It is important to remember that the tangent space is defined at points in the configuration space; i.e., for a single configuration space, there are many tangent spaces – one tangent space corresponding to each point in the configuration space. There is a term for the collection of tangent spaces for a surface; this is called the tangent bundle. Wikipedia has some nice pictures of tangent bundles for one-dimensional surfaces.
Let’s look at an example, a 2R planar arm. We have
\(\left( \begin{array}{c} x(t) \\ y(t) \end{array} \right) = \left( \begin{array}{c} l_1 \cos \theta_1(t) + l_2 \cos (\theta_1(t) + \theta_2(t)) \\ l_1 \sin \theta_1(t) + l_2 \sin(\theta_1(t) + \theta_2(t)) \end{array} \right)\)
What is the relationship between how fast the joints are moving (\(\dot q\)), and how fast the point \((x, y)\) is moving? We’ll start by taking a time derivative of each side. We have to use the chain rule, since the angles are functions of time. Remember that
\(\frac{d}{dt} f (\theta (t)) = \frac{df}{d\theta} \frac{d\theta}{dt}.\)
The chain rule has a simple physical explanation: we want to know how fast f changes with time; we know how fast f changes with \(\theta\), and we know how fast \(\theta\) changes with time.
\(\left( \begin{array}{c} \dot x(t) \\ \dot y(t) \end{array} \right) = \left( \begin{array}{c} -l_1 \sin \theta_1(t) \dot \theta_1(t) - l_2 \sin (\theta_1(t) + \theta_2(t)) (\dot \theta_1(t) + \dot \theta_2(t)) \\ l_1 \cos \theta_1(t) \dot \theta_1(t) + l_2 \cos(\theta_1(t) + \theta_2(t)) (\dot \theta_1(t) + \dot \theta_2(t)) \end{array} \right)\)
\(\dot \theta_1\) and \(\dot \theta_2\) are the joint velocities, while the other terms describe the geometry of the robot at a particular configuration. Let’s collect the velocity terms:
\(\left( \begin{array}{c} \dot x(t) \\ \dot y(t) \end{array} \right) = \left( \begin{array}{c} \dot \theta_1(t) (-l_1 s_1 - l_2 s_{12} ) - \dot \theta_2(t)) l_2 s_{12} \\ \dot \theta_1(t) ( l_1 c_1 + l_2c_{12}) + \dot \theta_2(t) l_2 c_{12} \end{array} \right)\)
We could write this in vector form:
\(\left( \begin{array}{c} \dot x(t) \\ \dot y(t) \end{array} \right) = \left[ \begin{array}{cc} (-l_1 s_1 - l_2 s_{12} ) & - l_2 s_{12} \\ ( l_1 c_1 + l_2c_{12}) & l_2 c_{12} \end{array} \right] \left( \begin{array}{c} \dot \theta_1(t) \\ \dot \theta_2(t) \end{array} \right)\)
The matrix is called the Jacobian matrix, and it transforms between velocities in joint space and velocities in the workspace. Notice that it is only a function of the configuration: so, given a configuration, the workspace velocity is related linearly to the joint space velocity.
Also notice that for our example, upper left element of the Jacobian happens to be the partial derivative of \(l_1 \cos \theta_1(t) + l_2 \cos (\theta_1(t) + \theta_2(t))\), the expression for x as a function of \(q\), with respect to \(\theta_1\). In fact, observation tells us that the Jacobian for this system has the form
\(J(q) = \left[ \begin{array}{cc} \partial x / \partial \theta_1 & \partial x / \partial \theta_2 \\ \partial y / \partial \theta_1 & \partial y / \partial \theta_2 \end{array} \right]\)
If we could compute the elements of the Jacobian by just computing partial derivatives of the forwards kinematics, that would be much easier than using the chain rule and factoring out \(\dot \theta\)s! In fact we can, and this is a consequence of the multivariable chain rule from vector calculus:
Let \(f(\theta_1(t), \theta_2(t), \theta_3(t), ...)\) be a function of n variables \(\theta_i\), each of which is a function of some other variable \(t\). Then
\(\frac{df}{dt} = \frac{\partial f}{\partial \theta_1} \frac{d \theta_1}{dt} + \frac{\partial f}{\partial\theta_2} \frac{d \theta_2}{dt} +\frac{\partial f}{\partial \theta_3} \frac{d \theta_3}{dt} + \ldots\)
The multivariable chain rule is easy to remember if we think about some examples. For example, let f be a function of x, y, z. Maybe f is the air temperature at different points in a room. Then let x(t), y(t), z(t) be a trajectory through that room. How fast does the temperature change (df/dt) at some point on that trajectory? The first thing we do is compute a vector tangent to the trajectory: (dx/dt, dy/dt, dz/dt). Then we compute a vector that describes how much the temperature changes in each of the coordinate directions x, y, z: \((\partial f / \partial x, \partial f / \partial y, \partial f / \partial z)\) – this vector is known as the gradient of the temperature. So how fast does the temperature change? It’s the dot product (or projection) of the tangent vector with the gradient.
For our system, the \(x\) coordinate of the arm is a function of several variables, \(\theta_1, \theta_2\), each of which is a function of time. So \(\dot x\) can be computed by taking the partial derivative of the forward kinematics for \(x\) with respect to \(\theta_1, \theta_2\), and taking a dot product with the joint velocities. This is the effect of multiplying first row of the Jacobian times the joint velocity vector! The second row of the Jacobian corresponds to the \(y\) coordinate.
So, the Jacobian is a generalization of the multivariable chain rule to vector-valued functions. That may sound intimidating, but in practice it’s easy to remember: you have one vector (the position of the end effector) whose elements are functions of another vector (the configuration of the robot), and the configuration of the robot is changing with time. To compute the relationship between the time derivative of each vector, just multiply by a matrix of partials.
Matrix multiplication is linear: \(A (u + v) = A u + A v\). If we fix the robot arm at a particular configuration, the Jacobian can be computed explicitly, and is just a constant matrix. We have
\(\dot x = J\dot q\)
where \(x\) is the vector describing the location of the end effector. It follows that the effect of moving joint one with \(\theta_1 = 3\) and moving (at the same time) joint two with \(\theta_2 = 4\) is the sum of the effects of the independent motions.
This means that you can (approximately) measure the Jacobian experimentally, at least at a fixed configuration. Lock joint 2, and move joint 1 slightly. Measure the motion of the end effector. This is the first column of the Jacobian. Lock joint 1, and move joint 2 slightly. This is the second column of the Jacobian.
The idea of moving joints independently to determine columns of the Jacobian gives an idea about how to compute the Jacobian for serial revolute arms without computing any partial derivatives. The first column of the Jacobian tells how the velocity of the end effector depends on the motion of the first joint. Lock all of the joints except the first. Draw the line from the first joint to the end effector. Rotating the joint will move the effector in a direction that is 1) perpendicular to the joint axis, 2) perpendicular to the line you just drew, and 3) has magnitude proportional to the length of that line. If you take the cross product of the joint axis and the vector from joint 1 to the effector, you’ll get a vector with exactly that property, and this will be the first column of the Jacobian.
How about the second column? Fix all joints except the second, draw the line from the second joint to the effector, and take the cross product with the second joint.
Sometimes columns of the Jacobian are linearly independent, and sometimes they are not. The rank of the Jacobian is the number of linearly independent columns. Each column describes a direction in the workspace that the robot can move by changing one configuration variable.
Inverse differential kinematics involves solving the differential kinematics equation \(J \dot q = \dot x\) for \(\dotq\) to derive joint motions that give a desired workspace motion. There are three cases:
The Jacobian has more columns than rows, and more joints than workspace parameters. There are typically infinitely many solutions. A pseudo-inverse of the Jacobian can be used to compute one of these solutions. Numerical software can compute a pseudo-inverse using a singular-value decomposition of the matrix.
The Jacobian is square. There is typically exactly one solution.
Solving the equation \(\dot x = J \dot
q\) for \(\dot q\) gives that
solution. Inverting the Jacobian might work, but may be numerically
unstable; numerical linear algebra software will provide a more direct
solve method that uses some better method (e.g., a variant
of Gaussian elimination, or a singular-value decomposition of the
matrix).
The Jacobian has more rows than columns. For particular arm motions, there may be a solution; the pseudoinverse will find it. (This will occur if the \(\dot x\) is a linear combination of columns of the Jacobian.) For other arm motions, there is no solution. One can check by computing a solution for \(\dot q\) using the pseudoinverse, plugging that solution back into the forward equations, and computing the actual \(\dot x\) for that solution. If the actual value does not match the requested value, there is no solution.