Cartesian Interpolated Motion

Let's restate the problem. We want to move the end effector pose smoothly from pose A to pose B. What we're going to do now is to determine the pose of the end effector at each intermediate point. We have an interpolation function, its arguments are the initial pose, the final pose and a scalar variable S which vary smoothly from 0 to 1. The result of this function is the intermediate pose of the robot's end effector. For each of these intermediate poses we're going to apply the inverse kinematic function to determine the joint angles that corresponds to that particular value of S.

So in summary what we're doing is that in every time step we're going to interpolate the post using the top equation. Then we're going to perform inverse kinematics. This is a different algorithm than what we used in the previous case. Previously we interpolated the joint angles. In this case we're actually interpolating the Cartesian pose. We're going to carry on from where we left off from the previous example. What we're going to do now is Cartesian interpolated motion. I am going to create a new workspace variable TS. This is the interpolated Cartesian pose. I'm going to use the function ctraj for Cartesian trajectory I am going to phase in the initial pose, the final pose and the number of time steps, create a new variable in our workspace called TS. We can see that this is a three dimensional matrix, four rows, four columns and 50 planes. So we can imagine as a set of 4x4 matrices stacked one above the other. The final dimension indicates which particular time step we're talking about.

Let's have a look at the, all the rows and all the columns of the first element. That's the initial pose. We can look at the second pose and the third pose and so on. Now I can actually phase this three dimensional matrix into the inverse kinematic function. Inverse kinematics for a robot with 6 joints and a spherical wrist. I'm going to pass in TS. What it's going to do is to compute the joint angles for every pose in this matrix. So the result now QS, we can see is a matrix with 50 rows and 6 columns. It's computed, the inverse kinematics, for every single pose in the matrix TS. And we can go back to figure 1 and we can plot that. See what it looks like. This is the Cartesian interpolated motion. And coming back to figure 2 we can plot the joint angles corresponding to that Cartesian motion. We can see that they are broadly similar to the trajectory that we got from joint interpolated motion. They certainly start and end at the same values but the path along the middle of the trajectory, they follow is slightly different. That difference is enough to change the motion from being slightly curved in 3D space to being a straight line in 3D space.

This is the result of the Cartesian interpolation strategy. Here we can see the robot joint angles varying smoothly from the initial pose to the final pose over a period of two seconds.

If I compute the forward kinematics from each of the intermediate set of joint angles and plot the x coordinate versus the y coordinate. I can see now that the robot end effector is moving in a perfectly straight line from A to B.

If I now apply forward kinematic function to all the intermediate sets of joint angles, I can determine the pose of the robot end effector. If I plot the end effector x coordinate against its y coordinate, I can see that the end effector has now moved in a perfectly straight line from pose A to pose B.

Similarly when I look at the orientation of the end effector I can see that the pitch and yew angles are equal to 0 for the entire duration of the trajectory. So the tool orientation remains constant and the tool tip has moved in a straight line.

We achieved this just by changing the way we do the interpolation or interpolating Cartesian poses rather than robot joint angles. If we compare the two sets of joint angle trajectories, we can see that they're actually quite similar. There is actually not very much variation in the joint angles as a function of time necessary to achieve this straight line end effector motion versus the curbed end effector motion.