function [px, py, gx, gy] = position_5_link(z,P) 
%[px,py,gx,gy] = POSITION_5_LINK(Z,P)
% 
%FUNCTION:  This function computes the cartesian positions
%    of the CoM and tip of each link
%INPUTS: 
%
%
%OUTPUTS: 
%    px = [nLink X nTime] position of the end of each link
%    py = [nLink X nTime] position of the end of each link
%    gx = [nLink X nTime] position of the CoM of each link
%    gy = [nLink X nTime] position of the CoM of each link
% 
%NOTES:
%    This file was automatically generated by writePosition.m

g  = P.g ; %gravity
m1 = P.m(1); % Link 1 mass
m2 = P.m(2); % Link 2 mass
m3 = P.m(3); % Link 3 mass
m4 = P.m(4); % Link 4 mass
m5 = P.m(5); % Link 5 mass
l1 = P.l(1); % Link 1 length
l2 = P.l(2); % Link 2 length
l3 = P.l(3); % Link 3 length
l4 = P.l(4); % Link 4 length
l5 = P.l(5); % Link 5 length
I1 = P.I(1); % Link 1 moment of inertia about its center of mass
I2 = P.I(2); % Link 2 moment of inertia about its center of mass
I3 = P.I(3); % Link 3 moment of inertia about its center of mass
I4 = P.I(4); % Link 4 moment of inertia about its center of mass
I5 = P.I(5); % Link 5 moment of inertia about its center of mass
d1 = P.d(1); % Link 1 distance between center of mass and parent joint
d2 = P.d(2); % Link 2 distance between center of mass and parent joint
d3 = P.d(3); % Link 3 distance between center of mass and parent joint
d4 = P.d(4); % Link 4 distance between center of mass and parent joint
d5 = P.d(5); % Link 5 distance between center of mass and parent joint

th1 = z(1,:); 
th2 = z(2,:); 
th3 = z(3,:); 
th4 = z(4,:); 
th5 = z(5,:); 

nTime = length(th1); 
px = zeros(5,nTime);
py = zeros(5,nTime);
gx = zeros(5,nTime);
gy = zeros(5,nTime);

px(1,:) = l1.*cos(th1);
py(1,:) = l1.*sin(th1);
gx(1,:) = d1.*cos(th1);
gy(1,:) = d1.*sin(th1);

px(2,:) = l1.*cos(th1) + l2.*cos(th2);
py(2,:) = l1.*sin(th1) + l2.*sin(th2);
gx(2,:) = d2.*cos(th2) + l1.*cos(th1);
gy(2,:) = d2.*sin(th2) + l1.*sin(th1);

px(3,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3);
py(3,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3);
gx(3,:) = d3.*cos(th3) + l1.*cos(th1) + l2.*cos(th2);
gy(3,:) = d3.*sin(th3) + l1.*sin(th1) + l2.*sin(th2);

px(4,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4);
py(4,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4);
gx(4,:) = d4.*cos(th4) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3);
gy(4,:) = d4.*sin(th4) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3);

px(5,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5);
py(5,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5);
gx(5,:) = d5.*cos(th5) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4);
gy(5,:) = d5.*sin(th5) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4);


end 
