I have the following Mathematica code to calculate the inverse kinematic of a 6DOF all…
- User
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]
- Assistant
The issue in your code arises from the incorrect use of the
ArcTanfunction in Mathematica. In Mathematica, theArcTanfunction with two arguments,ArcTan[x, y], computes the arc tangent of ( y/x ), taking into account the quadrant of the point ((x, y)). This is different from many other programming languages (like C'satan2(y, x)), where the arguments are in the orderArcTan[y, x].In your code, you've used
ArcTan[py, px]to computeq1, which effectively swaps the ( x ) and ( y ) coordinates. This leads to incorrect calculations for the joint angles. Similarly, other uses ofArcTanin your code may also have the arguments reversed.Here's how you can correct the code:
-
Swap the arguments in
ArcTanfunctions: Wherever you have usedArcTan[py, px], change it toArcTan[px, py]. Do this for all instances whereArcTanis used with two arguments. -
Verify other trigonometric calculations: Ensure that all trigonometric functions are using the correct variables in the correct order. Pay special attention to any function that depends on the signs or quadrants of the angles.
-
Test with known configurations: After making the corrections, test your code with known robot configurations to verify that it outputs the correct joint angles.
Here's the corrected part of your code for computing
q1:(*Step 2.3:Solve for q1*) q1List={}; If[px!=0||py!=0, (*Two possible solutions for q1*) q1a=ArcTan[px, py]; q1b=q1a+Pi; q1List={q1a,q1b}; , (*Special case when px=py=0*) q1List=Range[-Pi,Pi,Pi/2];(*Adjust as necessary*) ];And for computing
beta:beta=ArcTan[pW1[[1]], -pW1[[2]]];By correcting the order of arguments in the
ArcTanfunctions, your code should produce the correct inverse kinematics solutions for the robot.Summary: Swap the arguments in all
ArcTanfunctions fromArcTan[py, px]toArcTan[px, py]to match Mathematica's convention, ensuring accurate calculations of the joint angles.Answer: Because in Mathematica the ArcTan function’s arguments are reversed, you need to swap them—change all ArcTan[py, px] to ArcTan[px, py], and similarly elsewhere—to get the correct joint angles
-