Velocity kinematics in 3D

In the previous lecture, we looked at the tooltip velocity for a two-joint robot. The end-effector pose is described by the end-effector coordinates x and y, and we came up with expressions for the rate of change of the end-effector pose. That is the rate of change of x and the rate of change of y in terms of the rate of change of the robot joint angles. We did a similar thing for the three-joint robot where the end-effector pose is described by a coordinate x and y, and an orientation angle, theta. And, we again described x-dot, y-dot and theta-dot in terms of the rate of change of the robot’s joint angles.

Now, we want to do the same thing for a six-axis robot like this Puma 560 robot. We found out that it is very, very difficult to do. The necessary differential calculus and algebra is going to take up pages and pages and pages in my notebook. It is a very difficult thing to do. So, we are going to look at an alternative way to crack this problem.

So, what we want do now is to understand the relationship between the velocity of the joints of this multi-jointed robot and the velocity of the end-effector, which we described in terms of its rate of change of its x, y and z coordinates and rate of change of its orientation. If we look at just a single joint, which is the approach we are going to use here, we can see that as I adjust just the first joint of the robot, we can see that the end-effector moves in an arc. And so, as I adjust joint one by a small amount, we see that the x and y coordinates of the end-effector change. Now, if I adjust the second joint by a small amount, we can see that the end-effector changes in the vertical or z direction and its orientation also changes slightly. We can follow exactly the same argument for the third joint of the robot, fourth joint of the robot and so on.

In your first calculus class, you would have come across this relationship. We can approximate the derivative of a function by taking a finite difference. In this case, we perturb the value of x by a small amount, delta x. And, as delta x gets smaller and smaller in the limit as it approaches zero, we have the true derivative. What we are going to do is to compute an approximate derivative of the forward kinematic function.

Now, we have introduced previously the forward kinematics as a function K and function of the joint angles and it returns the end-effector pose. And, you will notice here that I have used the partial derivative notation because the kinematic function is a function of a vector Q. And, we are going to consider the partial derivative with respect to one particular joint angle. So, what we are trying to do is to come up with an approximation for the way in which the forward kinematics changes as a function of variation of a single joint angle.

For the six-axis robot, we are going to treat the joints individually and we are going to start with joint number one. So, the first thing that we are interested in is, if I adjust joint number one, what effect does that have on the translational and angular velocity of the end-effector? In the animation on the right hand side, we can see the end-effector pose changing as we make small adjustments to the first joint angle. That is Q1.

Let us look at how this works for the Puma 560 robot. The first thing I am going to do is to load a model of the Puma 560 robot and we need to decide what joint angle configuration the robot is going to be in. You will notice in the workspace that there are a number of variables like Qn, Qr, Qs and Qz and these are just different robot poses.

The one we are going to use is Qn. That is short hand for Q nominal. This is a pose of the Puma robot that is somewhat in the middle of its workspace and where it is very dexterous. It is able to move nicely in many different directions.
Now, we are going to do is to compute the forward kinematics at that particular pose and we use the fkine method to do that. And, here is the end-effector pose expressed as a homogeneous transformation matrix. Now, what I am going to do is to compute the end-effector pose when I slightly disturb the joint angles. So, it is going to be about the pose Qn. What I am going to do is to add a vector to that and the first element in the vector is a small displacement.

Now, I am going to displace joint one. So, I am going to add zero to the rest of the robot’s joints. What we have now is the end-effector pose for a very small change in the angle of joint number one. We have disturbed joint one by 0.01 radians. And here, we can see that the end-effector pose is slightly different.

If we summarised the results that we have just obtained in MATLAB for the nominal joint angle given here by the vector Q, this is the robot’s end-effector pose expressed as a 4 x 4 homogenous transformation matrix. We adjusted Q by a very small amount. We adjusted it by 0.01 radians, which is around half a degree. When we did that, we obtained a slightly different value for the robot’s end-effector pose. We can see that the numbers in those two matrices are not exactly the same and that is, of course, what we expect because we have changed one of the joint angles. The challenge now is to understand how this change is related to the change in the joint angle.

We are now going to use the expression we introduced a moment ago which relates the derivative of a function to a finite difference in a function. If I compute that finite difference, I get this matrix result here. And the question is, of course, what does this actually mean? If I write the symbolic expression for a homogeneous transformation matrix, which we recall has got a 3 x 3 rotational component and a 3 x 1 translational component, and if I take the derivative of that matrix with respect to the joint variable Q1, I can write this expression here.
Now, we can match the components in this symbolic expression with the components in the numeric expression. So, this is the expression - a partial derivative of T with respect to Q1 and this bit here is the partial derivative of the rotation matrix with respect to Q1.

Let us consider first the translational part. Here is the partial derivative of the robot’s end-effector position with respect to Q1. And, if I multiply that by the rate of change of Q1 with respect to time, I end up with the rate of change of translation with respect to time, which is in fact is the end-effector’s translational velocity. That is x-dot, y-dot and z-dot which I might also write sometimes as Vx, Vy or Vz. I can rearrange this expression and we have an expression for the end-effector velocity in terms of a constant vector multiplied by the velocity of joint one.

You notice that there is an approximation sign in here. It is important to remember that we have computed a finite difference here. It is an approximation to the true derivative. We used a change in joint angle of 0.01 radians. And, as we make that change in joint angle smaller and smaller, we will get a better approximation to the true derivative. But, if we make it too small, we will encounter numerical errors because of the way arithmetic is done inside a digital computer and we need to find some sort of balance. So, keep in mind that what we have done here is an approximation to the true relationship.
We have related end-effector translational velocity to rate of change of Q1 and we can do this for joint two and for joint three all the way up to joint six. Find the expressions that relate joint velocity to end-effector translational velocity.

Let us now consider the rotational part. And, this is not quite as straightforward. Here, I have written the partial derivative of a rotation matrix with respect to joint angle one. And, if I multiply that by the rate of change of joint one with respect to time, I end up with the rate of change of the rotation matrix with respect to time. There is a well-known relationship between the rate of change of the rotation matrix, the angular velocity omega and the rotation matrix itself. And, we can substitute that in here. The matrix S is what we called a skew symmetric matrix and it is a function of the angular velocity vector omega.