% tutorial1.m
%
% Instantiating a planar 2-link manipulator using FMAT
% Created 06/2010 by Lael Odhner <lael.odhner@yale.edu>

% clear the workspace
clear;

% create a vector of symbolic coordinates for the configuration of the
% robot
q = make_q(2);

% Instantiate the robot
r = Robot(q);

% Create an origin for the robot.
Origin = HT(r, 't', [0; 0; 0]);

% Define the link and joint transformations
Link1_Prox = HT(Origin, 'rz', q(1));
Link1_Dist = HT(Link1_Prox, 't', [0.5; 0; 0]);
Link2_Prox = HT(Link1_Dist, 'rz', q(2));
Link2_Dist = HT(Link2_Prox, 't', [0.5; 0; 0]);

% Create some cylinders to represent the joints of the robot arm.
r.Add_DisplayObject('joint1', 'cylinder', [0.2 0.3 1.0], Link1_Prox, 0.1, 0.1, 0.15);
r.Add_DisplayObject('joint2', 'cylinder', [0.2 0.3 1.0], Link2_Prox, 0.1, 0.1, 0.15);

% define the center of each link so we can place a rectangular display
% object there.
Link1_Center = HT(Link1_Prox, 't', [0.25; 0; 0]);
Link2_Center = HT(Link2_Prox, 't', [0.25; 0; 0]);

% Add boxes for the links. 
r.Add_DisplayObject('link1', 'box', [0.2 0.3 1.0], Link1_Center, 0.5, 0.08, 0.1);
r.Add_DisplayObject('link2', 'box', [0.2 0.3 1.0], Link2_Center, 0.5, 0.08, 0.1);

% define a configuration, q1
q1 = [pi/2+1, -1];

% create a figure and plot the arm in configuration q1
figure(1);
clf;
subplot(1,2,1)
r.Draw(q1, 1);
axis equal;
subplot(1,2,2)
r.Draw(q1, 1);
axis equal;
view([170 45]);

% print the example figure to a file.
print -dpng -r72 example1.png