%FORWARDKINEMATICS  Forward Kinematics
%  Function for calculating forward kinematics of a 2 link planar arm
%
% Syntax:  [J1, J2] = ForwardKinematics(J0, l1, l2, theta1, theta2)
%
% Inputs:
%    J0 - 2-vector giving x y coords of the robot base
%    l1 - length of link 1
%    l2 - length of link 2
%    theta1 - angle of first joint
%    theta2 - angle of second joint relative to first link
%
% Outputs:
%    J1 - x y coords of the end of link 1
%    J2 - x y coords of the end of link 2 (end-effector)

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

function [J1, J2] = ForwardKinematics(J0, l1, l2, theta1, theta2)

	theta2 = theta1 + theta2;
    J1 = J0 + [l1*cos(theta1); l1*sin(theta1)];
    J2 = J1 + [l2*cos(theta2); l2*sin(theta2)];