I have the following Mathematica code to calculate the inverse kinematic of a 6DOF all revolute joints robot. I don't undestand what is wrong, because the code gives no error but the solution is wrong:
Clear[InverseKinematics]
InverseKinematics[px_,py_,pz_,quaternion_,params_]:=Module[{(*Variables*) qSolutions={},q1List,q2List,q3List,q4List,q5List,q6List,pW,nx,ny,nz,ox,oy,oz,ax,ay,az,R06,R03,R36,L2,L3,L4,phi,r,beta,gamma,eta,q1,q2,q3,q4,q5,q6,wristConfigs,elbowConfigs,shoulderConfigs,a1=a1/.params,a2=a2/.params,a3=a3/.params,d1=d1/.params,d4=d4/.params,d6=d6/.params},(*Step 2.1:Convert Quaternion to Rotation Matrix*) {qw,qx,qy,qz}=quaternion;
R06={{1-2 qy^2-2 qz^2,2 qx qy-2 qz qw,2 qx qz+2 qy qw},{2 qx qy+2 qz qw,1-2 qx^2-2 qz^2,2 qy qz-2 qx qw},{2 qx qz-2 qy qw,2 qy qz+2 qx qw,1-2 qx^2-2 qy^2}};
(*Extract orientation vectors*) {nx,ny,nz}=R06[[All,1]];
{ox,oy,oz}=R06[[All,2]];
{ax,ay,az}=R06[[All,3]];
(*Step 2.2:Compute Wrist Position*) pW={px,py,pz}-d6{ax,ay,az};
(*Step 2.3:Solve for q1*) q1List={};
If[px!=0||py!=0,(*Two possible solutions for q1*) q1a=ArcTan[py,px];
q1b=q1a+Pi;
q1List={q1a,q1b};,(*Special case when px=py=0*) q1List=Range[-Pi,Pi,Pi/2];(*Adjust as necessary*)];
(*Step 2.4:Solve for q2 and q3 for each q1*) q2List={};
q3List={};
For[i=1,i<=Length[q1List],i++,q1=q1List[[i]];
(*Transform wrist position to frame 1*) pW1=RotationMatrix[-q1,{0,0,1}].(pW-{a1,0,d1});
(*Planar arm calculations*) L2=a2;
L3=Sqrt[a3^2+d4^2];
r=Norm[pW1[[1;;2]]];
beta=ArcTan[-pW1[[2]],pW1[[1]]];
cosgamma=(L2^2+r^2-L3^2)/(2L2 r);
If[Abs[cosgamma]<=1,gamma=ArcCos[cosgamma];
(*Two possible solutions for q2*) q2a=Pi/2-beta-gamma;
q2b=Pi/2-beta+gamma;
q2List=Append[q2List,{q2a,q2b}];
(*Solve for q3*) L4=L3;(*Since L3 already includes d4 and a3*) phi=ArcCos[(a3^2+L4^2-d4^2)/(2a3 L4)];
coseta=(L2^2+L4^2-r^2)/(2L2 L4);
If[Abs[coseta]<=1,eta=ArcCos[coseta];
(*Two possible solutions for q3 corresponding to each q2*) q3a1=Pi-phi-eta;
q3a2=Pi-phi+eta;
q3List=Append[q3List,{{q3a1,q3a2},{q3a1,q3a2}}];,(*No solution for q3*) q3List=Append[q3List,{{},{}}];];,(*No solution for q2 and q3*) q2List=Append[q2List,{}];
q3List=Append[q3List,{{},{}}];];];
(*Step 2.5:Solve for q4,q5,q6*) For[i=1,i<=Length[q1List],i++,q1=q1List[[i]];
If[q2List[[i]]=!={},{q2a,q2b}=q2List[[i]];
{q3Options1,q3Options2}=q3List[[i]];
(*For each combination of q2 and q3*) For[j=1,j<=2,j++,q2=If[j==1,q2a,q2b];
q3Options=If[j==1,q3Options1,q3Options2];
For[k=1,k<=Length[q3Options],k++,q3=q3Options[[k]];
(*Compute R03*) R01=RotationMatrix[q1,{0,0,1}];
R12=RotationMatrix[q2,{0,1,0}];
R23=RotationMatrix[q3,{0,1,0}];
R03=R01.R12.R23;
(*Compute R36*) R36=Transpose[R03].R06;
(*Solve for q4,q5,q6*) (*Wrist singularity check*) sy=Sqrt[R36[[1,3]]^2+R36[[2,3]]^2];
If[sy>=1^-6,q5a=ArcTan[sy,R36[[3,3]]];
q5b=ArcTan[-sy,R36[[3,3]]];
(*Two possible solutions for wrist angles*) q4a=ArcTan[R36[[2,3]],R36[[1,3]]];
q4b=ArcTan[-R36[[2,3]],-R36[[1,3]]];
q6a=ArcTan[R36[[3,2]],-R36[[3,1]]];
q6b=ArcTan[-R36[[3,2]],R36[[3,1]]];
(*Collect all combinations*) AppendTo[qSolutions,{q1,q2,q3,q4a,q5a,q6a}];
AppendTo[qSolutions,{q1,q2,q3,q4b,q5b,q6b}];,(*Handle singularity*) q5=If[R36[[3,3]]>0,0,Pi];
q4=0;(*Arbitrary value*) q6=ArcTan[-R36[[1,2]],R36[[1,1]]];
AppendTo[qSolutions,{q1,q2,q3,q4,q5,q6}];];];];];];
(*Return the possible joint solutions*)qSolutions]