Problems solving a trigonometric equation with 2 unknowns

Hello,

So, I have 2 trigonometric equations, the unknowns here are q2 and q3.

The 2 equations are:

Py=2*cos(q1) * ( cos(q2+q3) + 3*cos(q2) )
Pz=2*sin(q2+q3) + 6*sin(q2) + 14

First I tried on paper to get some function q3=f(q2), and that's what I ended up with:

q3=atan((Pz/2-7-3*sin(q2))/(Py/2/cos(q1)-3*cos(q2)))

Then I tried to solve only the first equation, substituting q3 with that function of q2 like this:

solve((Pz/2-7)==(sin(q2+q3)+3*sin(q2)), q2);

And the answer was: "Warning: Possibly spurious solutions."

I've also tried this, as I've seen in some post here

eqns = [(2*cos(q1)*(cos(q2+q3)+3*cos(q2))) == Py, (2*sin(q2+q3)+3*sin(q2)) == 0];
S = solve(eqns,[q2 q3])

Unsuccesfully tho

In another attempt I wanted to do in Matlab the same thing I did on paper, that is to get some q3=f(q2) and substitute it afterwards in the equation, but I ended up with complex numbers, and that's not something I wanted because q2 and q3 are angles for my robot, and Py and Pz are coordinates of the robot, so I can't get complex angles.

eqn = sin(b+c)+3*sin(b) == z/2-7;
sol = solve(eqn, b)

In this case b=q2, c=q3 and z=Pz. I tried it for the second equation and I got complex numbers too.

In my last attempt I tried a more direct approach

Result=solve(2*cos(q1)*(cos(q2+q3)+3*cos(q2)) == Py,...
  2*sin(q2+q3)+6*sin(q2)+14 == Pz ,q2,q3)

But I ended up with complex numbers again and a very long solution too. By no means the result that you should get from a robot with that has the first rotation about the Z-axis and the last 2 about the Y-axis.

Answer 1

[Raynier Suresh]

Using “solve” you can solve the equations and using “simplify” command you can do algebraic simplifications of the result.

You can use the following code to solve your equation. In the result you can substitute the values for end effector (Py, Pz) and q1 to calculate q2 and q3. By substituting proper value for Py, Pz and q1 you may not get complex values.

syms q1 q2 q3 Py Pz 
eqns = [Py==2*cos(q1) * (cos(q2+q3) + 3*cos(q2)), Pz==2*sin(q2+q3) +6*sin(q2) + 14] 
vars = [q2 q3] 
result = solve(eqns,vars) 
q2 = simplify(result.q2) 
q3 = simplify(result.q3) 

Refer the below documentation links for more information:

Solve: https://www.mathworks.com/help/symbolic/solve.html?searchHighlight=solve&s_tid=srchtitle

Simplify: https://www.mathworks.com/help/symbolic/simplify.html

[Denis Emanuel Pop]

After I run this I get the following results:

q2=z

q3=2*(pi*l+l)

'z' and 'l' are symbolic variables, what do I do with them? I couldn't find any information in those sources you've mentioned about symbolic variables. q2 is a rotation about the X-axis, it can't be just a random value z, without any other variable involved.

[Raynier Suresh]

Unnecessary white spaces in the eqns is what causing this, please replace the eqns with the below one.

eqns = [Py==2*cos(q1)*(cos(q2+q3)+3*cos(q2)), Pz==2*sin(q2+q3)+6*sin(q2)+14]

The result i got was

q2 =
 
 2*atan(((-(132*cos(q1)^2 + Pz^2*cos(q1)^2 + Py^2 - 28*Pz*cos(q1)^2)*(180*cos(q1)^2 + Pz^2*cos(q1)^2 + Py^2 - 28*Pz*cos(q1)^2))^(1/2) - 168*cos(q1)^2 + 12*Pz*cos(q1)^2)/(228*cos(q1)^2 + Pz^2*cos(q1)^2 + 12*Py*cos(q1) + Py^2 - 28*Pz*cos(q1)^2))
-2*atan((168*cos(q1)^2 + (-(132*cos(q1)^2 + Pz^2*cos(q1)^2 + Py^2 - 28*Pz*cos(q1)^2)*(180*cos(q1)^2 + Pz^2*cos(q1)^2 + Py^2 - 28*Pz*cos(q1)^2))^(1/2) - 12*Pz*cos(q1)^2)/(228*cos(q1)^2 + Pz^2*cos(q1)^2 + 12*Py*cos(q1) + Py^2 - 28*Pz*cos(q1)^2))
 
 
q3 =
 
-2*atan((-(132*cos(q1)^2 + Pz^2*cos(q1)^2 + Py^2 - 28*Pz*cos(q1)^2)*(180*cos(q1)^2 + Pz^2*cos(q1)^2 + Py^2 - 28*Pz*cos(q1)^2))^(1/2)/(180*cos(q1)^2 + Pz^2*cos(q1)^2 + Py^2 - 28*Pz*cos(q1)^2))
 2*atan((-(132*cos(q1)^2 + Pz^2*cos(q1)^2 + Py^2 - 28*Pz*cos(q1)^2)*(180*cos(q1)^2 + Pz^2*cos(q1)^2 + Py^2 - 28*Pz*cos(q1)^2))^(1/2)/(180*cos(q1)^2 + Pz^2*cos(q1)^2 + Py^2 - 28*Pz*cos(q1)^2))

[Denis Emanuel Pop]

It works now, thank you for your help