inverse_kin()

% Sumit Tripathi
%The inverse kinamatics based on numerical approach
function inverse_kin()


  global theta1_g d1_g theta2_g d2_g;
  global x_ee_g y_ee_g T w;

T = 3;
x0=[.5,.5];
lb=[0,-2*pi];
ub=[2,2*pi];
q=fmincon(@my_zfun,x0,[],[],[],[],lb,ub);
theta1_g = q(1);
theta2_g = q(2);
w = sqrt(2);

Qinit=[d1_g,d2_g,theta1_g,theta2_g];
% pause(2)
options=odeset;
FLAG =2;
[t,Y] = ode45(@rprp_trace,[0:0.1:T],Qinit,options,FLAG);
size = length(t);
d1_g = Y(size,1);
d2_g = Y(size,2);
theta1_g = Y(size,3);
theta2_g = Y(size,4);
theta1_g = theta1_g*180/pi;
theta2_g = theta2_g*180/pi;
if theta1_g >=360
    theta1_g = theta1_g-360;
else
    theta1_g = theta1_g;
end

if theta2_g >=360
    theta2_g = theta2_g-360;
else
    theta2_g = theta2_g;
end

if theta1_g <=-360
    theta1_g = theta1_g+360;
else
    theta1_g = theta1_g;
end

if theta2_g <=-360
    theta2_g = theta2_g+360;
else
    theta2_g = theta2_g;
end

if d1_g <=6
    d1_g = d1_g+6;
else
    d1_g = d1_g;
end


if d2_g <=6
    d2_g = d2_g+6;
else
    d2_g = d2_g;
end


if d1_g >= 16
    d1_g = d1_g-16;
else
    d1_g = d1_g;
end


if d2_g >= 16
    d2_g = d2_g-16;
else
    d2_g = d2_g;
end
 return;