Robotics CITS3241

Exercise Sheet 9: Motion Rate Control - With Orientation Control.

This is the final lab sheet.

Modify your stanfordmrcntrl function so that it also takes as an argument a rotation transformation matrix, Tf, that specifies the final orientation of the end effector. (Tf is a 4x4 homogeneous matrix but we only use the 3x3 rotation component of it)

stanfordmrcntrl should also return the transformation matrix describing the final end effector pose (which may end up being slightly different from the desired pose).

The specification for the function now becomes:

% function [finaljoint, Taf] = stanfordmrcntrl(joint, offset, CP, Tf, dt)
%
% where:           Tf    - 4x4 homogeneous homogeneous rotation matrix
%                          specifying the final desired orientation of the
%                          end effector.
%                  Taf   - The actual transformation matrix describing the
%                          final end effector pose that was achieved.
%
% ** The orientation of the end effector is interpolated linearly between
% the start and end configurations. ie orientation velocity is constant
% through the whole trajectory **
%

To calculate the incremental orientation change required at each step I suggest that you write a function with the following specification. The code will correspond to the case used in invrotaxis for when the rotation between the two frames is very small.

% function dO = twitch(T1, T2)
%
% Function calculates incremental rotations about x y and z axes
% required to transform frame T1 to T2.  Rotations are calculated by forming
% dot products between  old and new pairs of orthogonal axes
% to get component of rotation perpendicular to the pair of axes.
%
% dtheta_x =  z_old . y_new    
% dtheta_y =  x_old . z_new
% dtheta_z =  y_old . x_new
%
% It is assumed that the two frames are close together

Write a m file called "motion.m" that sets up some control points, a final orientation frame, and then calls stanfordmrcntrl to drive your arm along an 'interesting' trajectory. (Surprise me!)

Hints

The general computation scheme in your motion rate control code should be as follows:

Use the stanford forward kinematics code with the initial set of joint angles to find the initial position and orientation of the arm and the Jacobian matrix.
Calculate the equivalent axis of rotation and angle between the initial and final end effector orientations using invrotaxis
Set up the control points for the Bezier curve so that they start at the robot's current position.
Work out the number of cubic curves that make up the total path
Calculate the number of steps per curve (= 1.0/dt), and the total number of steps

Enter the following loop

for each curve
 for i = 1:nsteps
  - Increment curve parameter t
  - Calculate where the new end effector position should be on the Bezier 
    curve for the current t value. 
  - Calculate change in end effector position required to move to new position.
  - Calculate what the end effector orientation should be 
    (= initial orientation rotated by 
            currentStep/TotalSteps*total_angle_change about rotation axis) 
  - Calculate change in end effector orientation required to move to new position.
    Use the twitch function to do this.  Note that this orientation change is 
    expressed in terms of the current end effector's coordinate frame
  - Transform this incremental orientation change to base coordinates. (This is
    needed because the Jacobian Motion Rate Control equations are expressed in
    terms of incremental movements decribed in *base* coordinates)

   When you transform the orientation change from end effector
   coordinates to base coordinates you have to premultiply by T06.  To
   do this the orientation change vector has to be made into a 4
   element homogeneous coordinate.  In doing this you should append a
   0 to the vector (not a 1) this way when the transformation matrix
   is applied to the orientation vector it is only rotated (and not
   translated by the 4th column of T06 as well).  Alternative
   approaches are to zero out elements T06(1:3,4) or to simply
   premultiply the orientation change by T06(1:3,1:3) - just the 3x3
   rotation part of the transform.  
  - Construct the overall incremental change vector from the desired position
    and orientation changes
  - Solve for the change in joint angles using the inverse Jacobian and update
    the joint angles.
  - Calculate the new end effector position and orientation, and Jacobian matrix
    by calling your forward kinematics code with the new joint angles (and draw 
    the arm).  You may want to have a pause statement here so that the arm is redrawn 
    in sync with each step. 
 end
end