%%% constraints for optimizing 3 joint inverse kinematics
function [ineq_violations,eq_violations]=constraints(p)

global x_d;
global l1 l2 l3
global h_axes link1 link2 link3 target

a1 = p(1);
a12 = p(1) + p(2);
a123 = p(1) + p(2) + p(3);

pos1 = [l1*cos(a1) l1*sin(a1)]';
pos2 = pos1 + [l2*cos(a12) l2*sin(a12)]';
pos = pos2 + [l3*cos(a123) l2*sin(a123)]';

% be at the target
eq_violations(1) = pos(1) - x_d(1);
eq_violations(2) = pos(2) - x_d(2);

ineq_violations=[];

end
%% final end


