Robotics CITS3241

Exercise Sheet 8: Rotation About Arbitrary Axes.

Refer to the Lecture notes and to "Robot Manipulators" by Richard Paul, pages 25-34

1) Write a function that meets the following specification:

% function T = rot(axis,theta)
%
% Function returns a 4x4 homogeneous transformation matrix
% describing the rotation about an arbitrary axis by angle theta.

Warning: your code should handle the case where axis is not a unit vector.

2) Write a function that inverts the arbitrary rotation transformation.

%  function [axis, theta] = invrotaxis(T)
%
% Function computes equivalent axis of rotation and angle change
% given a rotation transformation matrix T
%
% input:  T     - 4x4 homogeneous rotation matrix
%
% output: axis  - Unit 3 vector describing the axis of rotation.
%         theta - The angle of rotation (radians)

Rather than follow the technique described in the lecture notes you should implement a geometric solution as follows:

a) Calculate axis vector:

The vectors describing the change in the unit vectors describing the frame axis will be perpendicular to the rotation axis

Thus, if the axis vectors are i, j, k and i_new, j_new, k_new , then the cross product

   (i_new - i) x (j_new - j)

will give the direction of the rotation axis. However, if the rotation axis is co-incident with, say, j the result will be ill conditioned. The solution is to calculate the axis in all three possible ways

   (i_new - i) x (j_new - j) , 
   (j_new - j) x (k_new - k) ,
   (k_new - k) x (i_new - i)

and pick the one with the greatest magnitude as the most accurate axis definition. (note that in our case i = [1 0 0], j = [0 1 0] etc)

b) Calculate rotation angle:

Find coordinate axis (i, j or k) that is most perpendicular to the rotation axis and form the cross product with the new and old directions of this axis with the rotation axis. These two new vectors are perpendicular to the rotation axis and perpendicular to the new and original axis directions. The angle between these two vectors gives us the angle of rotation.

Eg if the i axis was most perpendicular to the rotation axis we calculate the following

  e = cross(axis,[1 0 0]);   % cross product with old i axis
  f = cross(axis,T(1:3,1));  % cross product with new i axis

Angle of rotation = atan2 of |e||f|sin(theta) and |e||f|cos(theta) - We can get these two quantities from the cross and dot products between these two vectors

 theta = atan2(norm(cross(e,f)), e*f');

Note however that the scaled sine of the rotation angle as calculated from

 norm(cross(e,f))

will always be positive, even though the angle might be negative. To obtain the correct sign of the rotation angle we form the dot product between the rotation axis and the cross product of e and f. e x f will be in the +ve or -ve direction of the rotation axis. If the dot product between the two is -ve then the angle must have been negative, hence

 theta = theta*sign(cross(e,f)*axis');  % correct sign for direction of axis

c) Finally, all of the above may have been in vain!

The rotation angle may be so small that all the calculations above are very ill-conditioned.

So, if the rotation angle calculated above is smaller than, say, 0.05 radians we can do the following:

Use dot products between old and new pairs of orthogonal axes to get component of rotation perpendicular to the pair of axes.

 dtheta_x =  k_old . j_new      
 dtheta_y =  i_old . k_new
 dtheta_z =  j_old . i_new

How does this work?. The dot product between old and new directions of the orthogonal axes equals the cosine of the angle between these axes. Noting that

            cos(pi/2 - theta) = sin(theta)

and looking at the diagram above for the calculation of dtheta_z we can see the following


     dtheta_z ~= sin(dtheta_z)
               = cos(pi/2 - dtheta_z)
               = j_old . i_new

Thus taking the dot product of changes in orthogonal axes gives us the sine of the angle change in the axes themselves. As sin(theta) is a good approximation to theta for small theta (eg. sin(0.1) = 0.0998) we end up directly obtaining the rotation components in the x, y and z directions.

Incremental rotations can be added directly and are considered to be commutative. Thus the three components dtheta_x, dtheta_y and dtheta_z can be considered to form the components of the (un-normalized) rotation axis, and the overall rotation angle is obtained from the magnitude of these three components.

Portfolio Note

Save listings of rot and invrotaxis for inclusion in your portfolio.