function?[q2d1q50q31q32q3q41q42q4]=funrobota（n1n2n3o1o2o3m1m2m3a1p1p2p3d0d2d3d4）
%function?[q2d1q50q31q32q3]=funrobota（n1n2n3o1o2o3m1m2m3a1p1p2p3d0d2d3d4）
%求解角度q2
[q21]=solve（‘m1+sin（q21）=0‘）;
q21=eval（q21）;
if?-pi/2????q2=q21;
end

%求解距離d1
if?abs（q2-pi/2）<0.05
????d11=p3+d4;
????elseif?abs（q2+pi/2）<0.05
????d11=p3-d4;
????else
d11=p3-tan（q2）*（a1-p1）;
end
if?d11????d1=d11;%判斷移動距離是否超過有效行程
end
%為求解角度q3先求解角度q5
[q51]=solve（‘cos（q2）*o1-sin（q2）*o3+sin（q51）=0‘‘q51‘）;
q51=eval（q51）;
q52=pi-q51;
%q53=q51+2*pi;
%q54=q52-2*pi;
if?-3*pi/2????q50=q51;
????%q501=q53;
??elseif?-3*pi/2????q50=q52;
????%q501=q54;