%PUMAMRCNTRL Puma Motion Rate Control
% Function for performing motion rate control of a puma 6 dof arm
% along a series of cubic Bezier curves.
%
% Syntax:  [theta, T] = pumamrcntrl(theta, param, CP, Tf, dt, drawmode)
%
% Inputs:
%    theta - a 6 element array of the initial joint angles.
%    param - [d1 l2 d4 d6] (The non-zero lengths and offsets.)
%    CP - a nx3 vector of n Bezier control points in 3-space.
%    Tf - 4x4 homogeneous homogeneous rotation matrix specifying the final
%         desired orientation of the end effector.
%    dt - increment in the parameter along the Bezier curve that is made
%         between successive movements.
%    drawmode - same as puma6dof.
%
% Outputs:
%    finaltheta - a 6 element array of the joint angles at the final
%                 position of the arm.
%    Taf - The actual transformation matrix describing the final end
%          effector pose that was achieved.
%
% Other m-files required: puma6dof.m, startCubicBez.m, invht.m,
%                         startCubicBez.m, invrotaxis.m, bezier.m, rot.m,
%                         twitch.m
%
% See also: PUMA6DOF

% Author: Travis Hydzik
% Last revision: 20 October 2004

function [theta, T] = pumamrcntrl(theta, param, CP, Tf, dt, drawmode)

    [T, J] =  puma6dof(theta, param, drawmode);
    CP = [ startCubicBez(T(1:3,4)', [0 0 0]); CP ]
    
    [N, M] = size(CP);
    
    Ti = T;
    S = invht(Ti)*Tf;
    
    %find rotation axis and angle
    [axis, omega] = invrotaxis(S);
    
    domega = omega/(N/(4*dt));
    omega = 0;
    
    for i = 1:4:N
        for t = dt:dt:1
            omega = omega + domega;
            dO = twitch(T, Ti*rot(axis, omega));
            dx = [ (bezier(t, CP(i:i+3,:)) - T(1:3,4)') (T(1:3,1:3)*dO)' ]';
            dtheta = J\dx;
            theta = theta + dtheta';
            [T, J] =  puma6dof(theta, param, drawmode);
        end
    end