%MOTION
% sets up some control points, a final orientation frame, and then calls
% pumamrcntrl
%
% Syntax:  motion()
%
% Other m-files required: pumamrcntrl.m, goThrough.m, stopCubicBez.m

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

function motion()

    CP = goThrough([], [-3 0 1.5], [0 4 0]);
    CP = goThrough(CP, [0 3 2], [4 0 0]);
    CP = goThrough(CP, [3 0 2.5], [0 -4 0]);
    CP = stopCubicBez(CP, [2 -2 4], [-3 0 0]);
        
    theta = [-1.5708    0.7844   -3.6295    0.0000    0.2965   -1.5708];
    
    param = [2 2 2 0.5];
    
    dt = .2;
    
    drawmode = [2 0 0];
    
    Tf = [ 1 0 0 0; 0 1 0 0;  0 0 1 0; 0 0 0 1];
    
    theta2 = pumamrcntrl(theta, param, CP, Tf, dt, drawmode);