| Link | angle - theta | offset - d | length - a | twist - alpha |
| 1 | variable | d1=0.5 | 0 | 90 |
| 2 | variable | 0 | a2=1.0 | 0 |
| 3 | variable | 0 | a3=0.8 | 0 |
From these parameters, the A matrices are the same as the first three of the Mitsubishi robot we studied in the last assignment.
Multiplying the A-matrices results in the following elements in T3 that give the position of the tip:
These elements are all we will be interested in here, since we have no joints that will affect or control the orientation of the frame at the tip. The position elements could easily have been found by inspection for this robot without resorting to the laborious multiplication of matrices.
See the section below for doing this symbolically using MATLAB.
The numerical evaluation of this Jacobian has been programmed in jacob3r.m. This takes a vector containing the current joint values and returns the 3X3 array.
Here is a part of a diary file:
>kinem3r
PART 3 : TIP SPEEDS:
% joint positions in degrees
tdeg =
0 0 90.0000 30.0000
0 0 0 60.0000
0 90.0000 90.0000 -40.0000
% joint rates in rad/sec
tdot =
1.0000 0 -1.0000 0.5000
0 1.0000 1.0000 -0.2000
0 -1.0000 1.0000 0.6000
% world velocity components
XDOT =
0 0 1.0000 -0.2577
1.8000 0 -1.6000 0.5739
0 1.0000 1.0000 0.2007
Each column in this output contains the data in one row of the table in the assignment. Note that I used values for joint rates in rad/sec. The first two columns are simple checks that everything is working properly - these velocities can be verified easily by simple geometry.
Continuing on with the MATLAB script, we get the following:
PART 4 : JOINT RATES are:
% joint positions (degrees)
tdeg =
0 30.0000 30.0000 30.0000 30.0000 30.0000 30.0000
0 30.0000 30.0000 10.0000 5.0000 2.0000 1.0000
0 90.0000 -30.0000 -10.0000 -5.0000 -2.0000 -1.0000
% desired velocities (m/s)
XDOT =
1 0 1 1 1 1 1
0 1 1 1 1 1 1
0 -1 0 0 0 0 0
% required joint rates (rad/sec)
tdot =
NaN 1.8583 0.2197 0.2051 0.2038 0.2034 0.2034
NaN -1.1160 -2.7321 -7.8666 -15.6734 -39.1417 -78.2715
NaN 1.1998 5.6896 17.5505 35.1906 88.0390 176.0959
It turns out that when theta_3 is zero, the Jacobian is singular (we'll see why a bit later) - hence the joint rate data in the first column is NaN. In fact, in this position, the robot is not capable of moving in the +X direction at all, so asking for a velocity of 1 is a bit silly. Asking for a velocity of -1 in the X direction leads to another interesting problem - the robot can retract from this position either with the elbow up or elbow down. This is called (in mechanism theory) a bifurcation point, since there are two possible ways to leave the configuration. In mechanism theory, an indicator of a bifurcation point is that the Jacobian of the mechanism is singular.
The last five columns show us that as the arm approaches a fully stretched out configuration that the required joint rates to achieve the same Cartesian velocity grow very large, very quickly. We will this see in more detail in part 6. What is happening is as the robot approaches this position, the Jacobian is getting closer to being singular and increasingly large joint rates are required.
This indicates that when theta_3 = 0, then J is singular. This is the outer surface of the work envelope for this robot and has the shape of a sphere with center at the shoulder joint. Another possibility is if theta_3 = +/- 180 degrees - the arm is folded back on itself. This is the inner surface of the work envelope - also a sphere centered on the shoulder joint. The final possibility involves the term in parantheses. This represents the radial distance of the tip from the world Z axis. Thus, if this goes to zero with the tip located anywhere on the Z axis, the Jacobian will be singular.
In the script file kinem3r.m, a starting position (x,y,z) can be set as well as a desired constant tip velocity and increment between positions examined. The start position and velocity given in the assignment were
Starting location:
X =
0.8000
0
0.6000
Tip Speed:
XDOT =
0
0.2000
0
The program increments the X vector by a given amount (I used [0; 0.025; 0], ie increments in the Y direction) until the inverse kinematics returns an error (caused by the desired point being too far away). This occurred for
Illegal position - too far away.
X =
0.8000
1.6250
0.6000
While the computation was going on, some data was stored to produce the following plot:
Note that as the tip approaches the edge of the work envelope, the joint velocities start to change very rapidly. It is also interesting to plot the condition number of the Jacobian as this is going on. We get the following plot:
We know that as a matrix approaches being singular, its condition number becomes very high. This is born out in the figure above.
I had a look at another trajectory. This starts with the initial data:
Starting location:
X =
0.1000
-1.0000
0.8000
Tip Speed:
XDOT =
0
0.2000
0
This path starts off to the -Y side of the robot and passes very close to the Z axis (only 0.1m away) at a height of 0.8m (0.3m above the shoulder) as it moves off in the +Y direction. Here are the computed joint velocities:
Notice that as the tip passes across the X axis (Y = 0), the body joint velocity becomes very high. Here is what happens to the condition number of the Jacobian:
Near Y=0, the condition number increases substantially - this is when the tip is coming very close to the inner surface of the work envelope, and the Z axis.
You might like to experiment with other trajectories to see what happens, for example, if the path crosses through the Z axis. Remember that the inverse kinematics routine invkin3r does not implement any joint limits. If there were limits on the shoulder and elbow joints, it is possible that as the tip passes through the Z axis, the body joint would have to flip through 180 degrees instantaneously - not a pleasant prospect!
First, symbolically define the forward kinematics. The MATLAB output (using the pretty command for "pretty-printing") is:
Pretty-print of Forward kinematics:
[ cos(t1) (a2 cos(t2) + a3 cos(t2 + t3)) ]
[ ]
[ sin(t1) (a2 cos(t2) + a3 cos(t2 + t3)) ]
[ ]
[ d1 + a2 sin(t2) + a3 sin(t2 + t3) ]
This symbolic array that gives the (x,y,z) location of the tip is defined as array w. The variables are defined in vector v. The Jacobian is generated using the following MATLAB commands:
% evaluate the Jacobian symbolically
J = jacobian(w,v);
disp('Pretty-print of the Jacobian:')
pretty(J)
This produces the following output:
[- sin(t1) (a2 cos(t2) + a3 cos(t2 + t3)),
cos(t1) (- a2 sin(t2) - a3 sin(t2 + t3)), - cos(t1) a3 sin(t2 + t3)]
[cos(t1) (a2 cos(t2) + a3 cos(t2 + t3)),
sin(t1) (- a2 sin(t2) - a3 sin(t2 + t3)), - sin(t1) a3 sin(t2 + t3)]
[0, a2 cos(t2) + a3 cos(t2 + t3), a3 cos(t2 + t3)]
With the Jacobian found, we can evaluate the determinant. Quite a few people did this on the assignment, but without any form of simplification. Note the following MATLAB commands:
% get the determinant of J
d = determ(J);
d2 = simplify(d); % perform a couple of simplifications
d3 = simple(d2);
% make a substitution for a trig identity MATLAB can't recognize
d4 = subs(d3,'-sin(t3)','cos(t2+t3)*sin(t2)-sin(t2+t3)*cos(t2)');
disp('Pretty-print of det J :')
pretty(d4)
This results in the following output:
Pretty-print of det J :
- a2 sin(t3) a3 (a2 cos(t2) + a3 cos(t2 + t3))
This expression agrees with the one given above. Now let's find the symbolic inverse of the Jacobian. Here is the MATLAB command sequence, including commands for simplification:
% get the inverse of the Jacobian
invJ = inverse(J);
inv1 = simple(invJ);
% make a substitution for an identity MATLAB can't recognize
inv2 = subs(inv1,'-sin(t3)','cos(t2+t3)*sin(t2)-sin(t2+t3)*cos(t2)');
disp('Pretty-print the inverse of the Jacobian:')
pretty(inv2)
And here is the MATLAB output:
Pretty-print the inverse of the Jacobian: [ sin(t1) cos(t1) ] [ - ------- ------- 0 ] [ %1 %1 ] [ ] [ cos(t1) cos(t2 + t3) sin(t1) cos(t2 + t3) sin(t2 + t3) ] [ -------------------- -------------------- ------------ ] [ sin(t3) a2 sin(t3) a2 sin(t3) a2 ] [ ] [ %1 cos(t1) %1 sin(t1) a2 sin(t2) + a3 sin(t2 + t3) ] [ - ------------- - ------------- - ---------------------------- ] [ sin(t3) a2 a3 sin(t3) a2 a3 sin(t3) a2 a3 ] %1 := a2 cos(t2) + a3 cos(t2 + t3)
It is immediately obvious from this that the inverse Jacobian will be singular if sin(t3) = 0 or if the term identified as %1 is 0. These are the same conclusions as we arrived at above.
The moral of the story here is that if you are going to use MATLAB's symbolic operations, be sure to make full use of the simplification utilities - don't leave a job half-done! It may also be necessary for you to intervene by providing your own simplification substitutions. Although MATLAB's symbolic processing capabilities are quite remarkable, they may not be capable of doing everything for you.
Send a message to roger.toogood@ualberta.ca