Inverting the Jacobian Matrix

We have just worked out a way to describe the relationship between the rate of change of its joint angles and the translational and angular velocity of the robot’s end-effector. Spatial velocity is a six-vector that comprises the translational velocity, v, and the angular velocity, omega. And, we wrote a relationship between spatial velocity of the end-effector and the velocity of the joint angles.

For a robot like this, the Jacobian matrix, J, is a 6 x 6 matrix. It’s square. So, we can rearrange this expression to obtain the robot joint angle velocity that we need in order to achieve a desired robot end-effector spatial velocity and we can do this unless the Jacobian is singular. That is its determinant is equal to zero.
 
Unfortunately, there are some circumstances where the Jacobian will be singular and one of the singularities occurs when joint angle five is equal to zero as shown here. If we rotate joint 4, we can see how the robot end-effector moves. If I move joint 6, we see that the end-effector motion is exactly the same; it’s that due to joint 4. So, if I rotate joints 4 and 6 in opposite directions, they cancel each other out. They are both moving, but the end-effector is not moving at all.
 
Following on from the earlier example, we can test the determinant of the Jacobian matrix for the joint angle configuration QN. And, here we can see the determinant, and it’s not equal to zero. It means I can then invert the Jacobian matrix and this is what the inverse Jacobian looks like. If we consider a different joint angle configuration, I’m going to look at the joint configuration Q0 which is where all the joint angles equal to zero. If I compute the Jacobian here and pass in QZ, we can see this is what the Jacobian looks like and the determinant in this particular case is equal to zero.
 
So, let’s say, I’ve got a robot control problem and I want the robot end-effector to have a very specific spatial velocity denoted by the Greek letter nu, then I can work out what joint velocity I need in order to achieve that. If I know the joint angles, and then I can compute the Jacobian matrix and invert it. And then, I move all the joints at that particular speed and the robot end-effector will move with the desired spatial velocity, nu. Now, the problem is that because the robot joint angles are changing, after a short while, the Jacobian will be no longer appropriate because the Jacobian is a function of the joint angles. So, I will need to recompute the Jacobian matrix.
 
If I was to write a computer algorithm to do this, it would be a type of discrete time algorithm. It would be computed at a number of discrete time steps, time step 1, time step 2, time step 3, and I’ll generalize that to time step k. And, the interval between time steps is given by the symbol delta T. So, at time step, k, I will take the joint angles that’s measured at time, k. I compute the Jacobian, invert it, multiply it by the desired spatial velocity and determine the joint angle velocity that I need to achieve. If I multiply the joint velocity by the time interval, delta T, then I obtain the change in joint angles over that time step. I add it to the current set of joint angles and I end up with the set of joint angles that I need to have at the next time step, at time k+1. And, then I tell my robot to move to that new set of joint angles and I repeat the process.
 
We are now going to look at resolved-rate motion control. And, I have already computed the Jacobian matrix for the set of joint angles which I call QN. Now, what I want to do is to specify the spatial velocity that I want my robot to have. I’m going to put that in to a variable called, nu, which is the Greek letter that I used to denote spatial velocity and I am going to ask for a spatial velocity of zero in x, zero in the y, one unit per second in the z direction and no angular velocity. And, I’m going to put that in to a column vector. So, there we have it. If I now multiply the inverse of the Jacobian by the desired spatial velocity, and what I have here is the joint angle velocity vector. This is the set of joint angle velocities that will give me the desired spatial velocity vector that’s a motion of one unit per second in the vertical or z direction.
 
We can see that it involves no motion at all for joints 1 and 4 and it does require motion of joints 2 and 3. That’s the shoulder and the elbow joint and also joint 5, one of the wrist joints. And, we can see that of all the joints, it is the elbow joint, joint 3 that has to move the fastest in order to achieve the desired motion.

Here is an animation that shows this in practice. I have commanded the robot to move vertically with a velocity of positive 1 in the world z direction. We observed that the robot end-effector moved that in constant velocity. But, if we look at how the joint angles change as a function of time, we see that they are not moving linearly.

We also know that some of the joint angles are increasing in velocity. And, you notice that as the robot got to the end of that motion, it started to look a little bit awkward. It can only move in the z direction for so long. Eventually, it’s not going to be able to reach the position that it needs to move to.