Mec.E. 555 Solution

Assignment #1 Solution

Problem #1

Here is a MATLAB diary file for the operations required in problem 1.

>%call routine to define object data
>objdat1
>%examine the object data
>% the object is
>P
P =
     0     0     0     3     3     3
     0     0     2     2     0     0
     0     1     0     0     1     0
     1     1     1     1     1     1

>% the number of vertices is
>nv
nv =
     6

>% the edge array is
>E
E =
     1     2
     2     3
     3     1
     1     6
     2     5
     3     4
     4     5
     5     6
     6     4

>% the number of edges is
>ne
ne =
     9

>%plot the object
>objplot(P,E,nv,ne)

Here is the object in the original position:

>%1. rotate 90 about world X axis
>P2 = rotx(pi/2)*P
P2 =
         0         0         0    3.0000    3.0000    3.0000
         0   -1.0000    0.0000    0.0000   -1.0000         0
         0    0.0000    2.0000    2.0000    0.0000         0
    1.0000    1.0000    1.0000    1.0000    1.0000    1.0000

>objplot(P2,E,nv,ne,'m')

Here is the object after Rotx(90):


>%2. translate -2 in world X direction
>P3 = transl(-2,0,0)*P2
P3 =
   -2.0000   -2.0000   -2.0000    1.0000    1.0000    1.0000
         0   -1.0000    0.0000    0.0000   -1.0000         0
         0    0.0000    2.0000    2.0000    0.0000         0
    1.0000    1.0000    1.0000    1.0000    1.0000    1.0000

>objplot(P3,E,nv,ne,'c')

Here is the object after Trans(-2,0,0)*Rotx(90):


>%3. rotate 90 about world Z
>P4 = rotz(pi/2)*P3
P4 =
    0.0000    1.0000    0.0000    0.0000    1.0000    0.0000
   -2.0000   -2.0000   -2.0000    1.0000    1.0000    1.0000
         0    0.0000    2.0000    2.0000    0.0000         0
    1.0000    1.0000    1.0000    1.0000    1.0000    1.0000

>objplot(P4,E,nv,ne)

Here is the object after Rotz(90)*Trans(-2,0,0)*Rotx(90):

>% the composite transform is
>T = rotz(pi/2)*transl(-2,0,0)*rotx(pi/2)
T =
    0.0000    0.0000    1.0000    0.0000
    1.0000    0.0000    0.0000   -2.0000
         0    1.0000    0.0000         0
         0         0         0    1.0000

>%object obtained using composite transform
>P4b = T*P
P4b =
    0.0000    1.0000    0.0000    0.0000    1.0000    0.0000
   -2.0000   -2.0000   -2.0000    1.0000    1.0000    1.0000
         0    0.0000    2.0000    2.0000    0.0000         0
    1.0000    1.0000    1.0000    1.0000    1.0000    1.0000

>%compare this with previous
>P4 == P4b
ans =
     1     1     1     1     1     1
     1     1     1     1     1     1
     1     1     1     1     1     1
     1     1     1     1     1     1

>% an exact match!


>% PART C

This is the right-to-left interpretation of the composite transform given above. First rotate 90 degrees around object z axis to get this:

Now translate 2 units along the object -x axis (world -Y) to get this:

Now rotate 90 degrees about the object x axis (world Y) to get this:

This results in exactly the same final position as before, as it should.

>% PART D
>% a possible inverse transform is (by inspection)
>Tinv = transl(3,0,0)*rotz(-pi/2)*roty(-pi/2)*transl(0,-1,0)
Tinv =
    0.0000    1.0000    0.0000    2.0000
    0.0000    0.0000    1.0000    0.0000
    1.0000         0    0.0000         0
         0         0         0    1.0000

>% try to put the prism back using this inverse transform
>P5 = Tinv*P4
P5 =
         0         0    0.0000    3.0000    3.0000    3.0000
    0.0000    0.0000    2.0000    2.0000    0.0000    0.0000
    0.0000    1.0000    0.0000    0.0000    1.0000    0.0000
    1.0000    1.0000    1.0000    1.0000    1.0000    1.0000

>% is this the same as original?
>abs(P5 - P) < .0001
ans =
     1     1     1     1     1     1
     1     1     1     1     1     1
     1     1     1     1     1     1
     1     1     1     1     1     1

>% it appears to be the same, within round-off error.

>%PART E
>% transform of part B was
>T
T =
    0.0000    0.0000    1.0000    0.0000
    1.0000    0.0000    0.0000   -2.0000
         0    1.0000    0.0000         0
         0         0         0    1.0000

>% the inverse of this is
>TI = inv(T)
TI =
    0.0000    1.0000         0    2.0000
    0.0000    0.0000    1.0000         0
    1.0000    0.0000    0.0000         0
         0         0         0    1.0000

>% the transform obtained manually in part D was
>Tinv
Tinv =
    0.0000    1.0000    0.0000    2.0000
    0.0000    0.0000    1.0000    0.0000
    1.0000         0    0.0000         0
         0         0         0    1.0000

>% are these the same?
>abs(TI - Tinv) < .0001
ans =
     1     1     1     1
     1     1     1     1
     1     1     1     1
     1     1     1     1

>% They are the same.

Problem #2

The three coordinate frames can be determined by inspection by noting the components of unit vectors in the x-y-z directions of each frame. These unit vectors make up the first three columns of the transform matrix. The final column is the location of the origin of the frame. The frames are:

Problem #3

The Denavit-Hartenberg parameters of the SCARA robot shown in the figure are:

Link angle - theta offset - d length - a twist - alpha

1 variable 0 a1 0

2 variable 0 a2 180

3 variable 0 0 0

4 0 variable 0 0

Note the 180 degree twist of link 2. This is to place the Z2 axis pointing downward so that theta-3 is in the sense indicated in the figure.

The A matrices for the robot are:

Multiplying these out results in the following forward kinematics (after much tedious manual labour and many sheets of scrap paper!):

There is a notational variation here - notice that C123 and S123 represent the sine and cosine of (theta_1 + theta_2 - theta_3). This occurs because we wanted to have theta_3 rotation positive in the opposite direction from the first two joints.

Note that the n, o, and a columns are orthogonal, as they should be, and each have a magnitude of 1. Since the inverse of this matrix must also be legal, the rows of the 3X3 rotation part of the matrix are also orthogonal and have a magnitude of 1.

This matrix can also be verified by substituting various combinations of joint angles (use 0 and +/- 90 degrees for convenience) and evaluating the T4 matrix. A quick sketch of the robot will indicate whether or not the final frame is in the correct position and orientation. This should be done at a number of positions to check all possible arrangements. Be careful when verifying that you check all parts of a term: it is easy to zero out part of a term when the other part is in error (for example: if you have C1S2 instead of S1S2 and you only check for S2 = 0, you will not catch the error). It is a good idea to verify a position where none of the terms vanish. (But don't check with angles of pi/4 since sine and cosine will give the same result.)

Playing with the MATLAB Robotics Toolbox

Just for fun, let's see what the Toolbox forward kinematics function does. This function is fkine(dh,q) where dh is an nX5 array with n=number of joints. Each row corresponds to one link and gives the DH parameters for that link (alpha, a, theta, d, type) where type=0 indicates a revolute joint and type=1 indicates a prismatic joint. If all joints are revolute, the last column of the dh array can be left off. The nX1 vector q contains the joint values, for both revolute and prismatic joints.

The symbolic forward kinematics has been programmed in the script file scarafk.m. The DH parameters can be set by running the script scaradh.m. Both these assume values a1=3 and a2=2.

It is interesting to measure the number of floating point operations required for the forward kinematics (flop count) using the two methods (fkine and the symbolic solution obtained above).

Here is a diary of a MATLAB session:

>% first, generate some joint values at random
>q = rand(4,1)
q =
    0.0077
    0.3834
    0.0668
    0.4175

>% get the SCARA DH parameters
>scaradh
>dh
dh =
         0    3.0000         0         0         0
    3.1416    2.0000         0         0         0
         0         0         0         0         0
         0         0         0    1.0000    1.0000

>% run the Toolbox forward kinematics
>flops(0)   % reset the flop counter to zero
>Tbox = fkine(dh,q)
Tbox =
    0.9479    0.3186    0.0000    4.8489
    0.3186   -0.9479    0.0000    0.7855
    0.0000    0.0000   -1.0000   -0.4175
         0         0         0    1.0000

>flops
ans =
   552

>% now run the direct programmed forward kinematics
>flops(0)   % reset the flop counter to zero
>Tdirect = scarafk(q)
Tdirect =
    0.9479    0.3186         0    4.8489
    0.3186   -0.9479         0    0.7855
         0         0   -1.0000   -0.4175
         0         0         0    1.0000

>flops
ans =
    18

>% try another set of joint values
>q = rand(4,1)
q =
    0.6868
    0.5890
    0.9304
    0.8462

>% direct kinematics
>Tdirect = scarafk(q)
Tdirect =
    0.9410    0.3385         0    2.9015
    0.3385   -0.9410         0    3.8157
         0         0   -1.0000   -0.8462
         0         0         0    1.0000

>% Toolbox forward kinematics
>Tbox = fkine(dh,q)
Tbox =
    0.9410    0.3385    0.0000    2.9015
    0.3385   -0.9410    0.0000    3.8157
    0.0000    0.0000   -1.0000   -0.8462
         0         0         0    1.0000

You can see that fkine() gives exactly the same result as the programmed forward solution. Note that fkine() requires about 30 times as many operations - it is wasting a lot of time multiplying all the matrices together. This is the main attraction of obtaining direct symbolic solutions.

Questions?

Send a message to roger.toogood@ualberta.ca

Return to Assignments List

Link	angle - theta	offset - d	length - a	twist - alpha
1	variable	0	a1	0
2	variable	0	a2	180
3	variable	0	0	0
4	0	variable	0	0