机器人运动学逆问题反解程序

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;