%% 单腿逆运动学模型——肘式
function [q1,q2,q3]=invar_kinamic_z(a,b,c,x,y,z)
if z==0
if y>0
q3=pi/2;
elseif y<0
q3=-pi/2;
else
q3=0;
end
else
q3 = atan(y/(-z));
end
l=sqrt(x^2+(y-a*sin(q3))^2+(z+a*cos(q3))^2);
q2=-acos((b^2+l^2-c^2)/(2*b*l))+asin(x/l);
q1=pi/2-acos((b^2+c^2-l^2)/(2*b*c));
end
