Hello guys, I need to solve this system of nonlinear symbolic equation; the variables known are X,Y,Z and I already know the solution for q1; so I just need to find q2 and q3.

%% Inverse kinematics 3Dofs + Constraint for q4 + free q5

syms q1 q2 q3 q4 q5 d1 d5 a2 a3 a4 X Y Z

eq1 = cos(q1)*(a4 + a3*cos(q2 + q3) + a2*cos(q2)) == X

eq2 = sin(q1)*(a4 + a3*cos(q2 + q3) + a2*cos(q2)) == Y

eq3 = d1 - d5 - a3*sin(q2 + q3) - a2*sin(q2) == Z

eq4 = q1 == atan2(Y,X)

[Bjorn Gustavsson]

Equations eq1 and eq2 lets you solve for q2. When I try that those solutions doesn't seem to be equal. That "might" be a problem going forward and solving for q3.

For this type of problem I'd like to try to get numerical solutions or just plot the difference between LHS and RHS for values of the unknowns for the three equations just to get a feel for what they look like and where solutions might lie, if there are multiple curves that need to intersect at one particular point and/or whatever it might be.

[Fancesco Cesarano]

The problem is for a robot. there are multiple solutions to be discarded later, but I don't know how to find them all by solving the system symbolically

Answer 1

[Torsten]

syms d1 d5 a2 a3 a4 q1 q2 q3 X Y Z
% Square eq1 and eq2, add both sides of the equations and take sqrt 
% Check for other solution when you replace sqrt(X^2+Y^2) by -sqrt(X^2+Y^2)
eq5 = a3*cos(q2 + q3) == sqrt(X^2 + Y^2) - a4 - a2*cos(q2);  
% Square eq5 and eq3' = a3*sin(q2 + q3) == d1 - d5 - a2*sin(q2) - Z and add both sides
eq6 = a3^2 == (sqrt(X^2 + Y^2) - a4 - a2*cos(q2))^2 + (d1 - d5 - a2*sin(q2) - Z)^2
% Solve for q2
q2 = solve(eq6,q2)

Now insert q2 in your equation 3 and solve for q3.

[Fancesco Cesarano]

Thank you!!