Inverse Kinematics and Robot Motion

Let's now consider the important and very realistic problem of moving a robot end effector from one pose to another.  From side A to side B and we want to move it smoothly.  Well we can determine the joint angles that correspond to pose A and we can determine the joint angles that correspond to pose B, simply by applying the inverse kinematics that we've already studied.

Now that we've got these two sets of joint angles, the initial set of joint angles and the final set of joint angles, all we need to do is move smoothly from one set to the other and we've previously discussed how to do this, how to interpret vector quantities smoothly over time.

Okay, we're back in MATLAB and first of all I'm going to define model for my Puma 560 robot.  And now I'm going to define a couple of poses and I am going to orientate this particular pose so that the Z axis is pointing straight down, as if the robot was working above a table. And the destination pose is going to be like this, minus 0.20 and I'm going to orientate it so that the Z axis is parallel to the XY plane, rotated around the X axis by Pi on 2.

Now I can determine the joint angles for the initial pose ikine6s, I'm going to pass in the initial pose and that's the joint angles and for the final pose I will do the same thing.  Now I can compute a trajectory that goes from the initial set of joint angles to the final set of joint angles in 50 steps, put a semi colon on the end so it doesn't print that, but the result will be a 50 by 6 matrix, we can see here, 50 rows, one row per time step and each column represents the joint angle evolving over time.  Now I can plot the robot making that particular motion.  I can animate it by passing in the trajectory and here we see the robot doing the motion; it's moving from left to right and the tool is rotating. 

If I start a new figure I can plot the joint angles and see how they evolved over time. And we can see that they vary smoothly as a function of time over the 50 time steps.  Now we can look at the joint angle trajectories and we can see that the joint angles vary smoothly along the trajectories, starting with the joint angles necessary to achieve pose A and ending with the set of joint angles necessary to achieve pose B and this smooth motion occurs over a period of two seconds.  Given that I have the joint angles at every point along the trajectory; it's relatively straight forward to compute the forward kinematics for each of those intermediate sets of joint angles. And here I can plot the end effector Cartesian position, that's it's X, Y and Z co-ordinate as a function of time and we can see that those co-ordinates vary smoothly as a function of time, which is very, very desirable.

However if I plot the path on the XY plane, we can see that it starts at A and goes to B but it doesn't move in a straight line in the XY plane.  When I'm computing the kinematics for every set of intermediate joint angles, I can also determine the end effector orientation and here I've plotted the roll, pitch and yaw angles of the end effector, as it moves from A to B.  We can see that the pitch and yaw angles are zero at the beginning of the trajectory and at the end of the trajectory, but in the middle of the trajectory we can see that the pitch angle is not equal to zero.  By tackling the problem in this particular form, we've really only applied two constraints. 

We've said what we want to happen at A and what we want to happen at B, but we haven't said anything about what happens in between.  And the result of this approach is that the end effector has not followed a straight line path in the world and the orientation of the end effector has deviated from the desired orientation in the mid points of the trajectory.  In the next section we'll look at a way we can remedy this.