xcurrent = 0; %Starting position (x)

zcurrent = 0; %Starting position (y)

xGoal = .5; %Target coordinates
zGoal = .5; %Target coordinates
theta1 = 0.1; %Angle of first
DOF
theta2 = 0.1; %Angle of second
DOF
theta3 = 0.1; %Angle of third
DOF
xchange = xcurrent - xGoal %Current
distance from goal in x coordinates
zchange = zcurrent - zGoal %Current
distance from goal in z coordinates
% input for Length of links
prompt1 = 'length of segment 1 ';
prompt2 = 'length of segment 2 ';
prompt3 = 'length of segment 3 ';
l1 = input(prompt1)
l2 = input(prompt2)
l3 = input(prompt3)
%LOOP STARTS

while ((xchange > .02 || xchange < -.02) || (zchange < -.02 || zchange > .02))
%input variable
for jacobian matrix
in1 = l1*cos(theta1);
in2 = l2*cos(theta1+theta2);
in3 = l3*cos(theta1+theta2+theta3);
in4 = -l1*sin(theta1);
in5 = -l2*sin(theta1+theta2);
in6 = -l3*sin(theta1+theta2+theta3);
jacob = [in1+in2+in3, in2+in3, in3; in4+in5+in6, in5+in6
, in6; 1,1,1];
invJacob = inv(jacob);

xcurrent = l1 * sin(theta1) + l2 * sin(theta1+theta2) +

l3 * sin(theta1+theta2+theta3)
zcurrent = l1 * cos(theta1) + l2 * cos(theta1+theta2) +
l3 * cos(theta1+theta2+theta3)
%incrementation values
xIncrement = (xGoal - xcurrent)/100;
zIncrement = (zGoal - zcurrent)/100;
increMatrix = [xcurrent; zcurrent; 1];
%incremented values
change = invJacob * increMatrix;
theta1 = theta1 + change(1)
theta2 = theta2 + change(2)
theta3 = theta3 + change(3)
xcurrent = l1 * sin(theta1) + l2 * sin(theta1+theta2) + l3 * sin(theta1+
theta2+theta3)
zcurrent = l1 * cos(theta1) + l2 * cos(theta1+theta2) + l3 * cos(theta1+
theta2+theta3)
xchange = xcurrent - xGoal
zchange = zcurrent - zGoal
end