fwd_kin(q,x)

function X = fwd_kin(q,x)
% Given a position in the configuration space, calculate the position of
% the end effector in the workspace.
%
% Author: James T. Allison, Assistant Professor, University of Illinois at
% Urbana-Champaign
% Date: 7/30/12
% Copyright 2012, James T. Allison

% example input:
% q = [pi/4 -pi/8]';
% x = [0.5 0.6]';


% configuration space coordinates:
q1 = q(1);              % theta 1
q2 = q(2);              % theta 2

% manipulator parameters:
l1 = x(1);              % link 1 length
l2 = x(2);              % link 2 length

X = [l1*cos(q1) + l2*cos(q1+q2)
    l1*sin(q1) + l2*sin(q1+q2)];