Solving a System of Trig Equations

I have a system of two equations both with trig functions in them and I'm trying to solve for one variable. In this case, the equations are:

x = a2*cos(theta1+theta2) + a1*cos(theta1)

y = a2*sin(theta1+theta2) + a1*sin(theta1)

In these equations x, c, a1, a2, and theta2 are all known. The only unknown is theta1. I've tried several techniques and either I'm choosing the wrong methods are I'm doing them wrong. How can I solve this system of equations for theta1?

[Walter Roberson]

Are there any constraints? For example are some of the values certain to be real valued?

Each of the two equations can be solved for theta1 in closed form, getting a formula involving log and a bunch of complex exponentials. However with there only being one free variable it is not obvious that the two can be equated. They are closed forms, no choice of branches - no Lambert W function for example.

[Michael Boyte]

I know the answer for theta1 must be a real number. These equations define a real, physical system. What I can do is define what all the variables are, including theta1 and I get some real value for these two equations. My goal is to basically reverse engineer what theta1 must be given all the other values. In one test example, I defined all the values and gave theta1 a value of pi/4. That gave me some real value that I set each of these equations to. I then acted like theta1 was unknown and tried to solve for it, but I can't seem to do it.

[Sam Chak]

@Michael Boyte

I'm trying to visualize what kind of 'thing' it will produce.

Will there be multiple solutions in the interval ?

theta = linspace(-pi, pi, 61);
[Theta1, Theta2] = meshgrid(theta);
a1 = 1;
a2 = 1;
X = a2*cos(Theta1 + Theta2) + a1*cos(Theta1);
Y = a2*sin(Theta1 + Theta2) + a1*sin(Theta1);
surf(X, Y)
xlabel('x'), ylabel('y')

[Michael Boyte]

This is part of an equation to solve for the joint angles of a serial robot. I know where the end of the robot is. The x- and y-coordinates of the end of the robot are the two equations I have. The z-coordinate has already been solved and plays no role in this part. There are only two revolute joints in this robot. So when I have a given position, which again are represented by the two equations I have, there are two sets of joint angles that can give me that position. I am able to use another equation, which does not have theta1 in it, to solve for two different theta2's. Each of those theta2s should have a corresponding theta1 that when combined, will give me the position. For example, in one of my tests, I gave theta1 a value of 45 degrees and theta2 a value of 60 degrees. Those values, along with the constants, gave me a particular position as a real value. When I used that real value, I ended up with two possible theta2's, the 60 degrees I gave it, and -60 degrees. I know logically that a solution for theta1 give theta2 = 60 degrees is 45 degrees. For theta2 = -60, theta1 must be -45 degrees. Unfortunately I can't seem to solve for theta1 generally. Did that answer your question?

[Sam Chak]

Thanks for your explanation, @Michael Boyte. No wonder that the problem looks difficult. It's an Inverse Kinematics problem. Is it possible to make use of the inverseKinematics() solver? The solver requires the Robotics System Toolbox.

If you have the Symbolic Math Toolbox, try solving for as advised by @Torsten in the Answer.

Answer 1

[Sam Chak]

Hi @Michael Boyte

Back to the basics when solving the inverse kinematics problem. The kinematics of a 2-link robot arm (where the lengths of the links {} are known), is governed by these two trigonometric equations:

... ... ...

... ... ...

Given the desired end-effector pose and the second joint configuration is pre-determined, we want to calculate the first joint configuration .

Step 1:

So, we start with . From the angle sum trigonometric identities, can be rewritten as

... ... ...

Similarly, can also be rewritten as

... ... ...

Now, we can put and in a more compact form ... the MATRIX form:

Step 2:

The matrix form is a System of Linear Equations. There are a few ways to solve the system and MATLAB can easily get this done. For educational purposes, let's continue to derive the formulas to calculate the first joint configuration .

The determinant can be simplified to

At this point, we can write the two formulas as

... ... ...

... ... ...

Step 3:

Either one of the formulas can be used to calculate the first joint configuration by taking the inverses of the cosine and sine.

... ... ...

... ... ...

Note that the real solutions only exist if the following conditions are met:

... ... ...

... ... ...

with the obvious and .

Example:

A 2-link robot arm, where the lengths are and , is used in this example. Given the desired end-effector pose and the second joint configuration is pre-determined, the first joint configuration can be calculated using the formula or .

a1      = 2/sqrt(3);                % length of 1st link
a2      = (sqrt(3) - 1)/sqrt(3);    % length of 2nd link
x       = 1;                        % desired x-coordinate of end-effector pose
y       = 1;                        % desired y-coordinate of end-effector pose
theta2  = pi/3;                     % 2nd joint config
den     = a1^2 + a2^2 + 2*a1*a2*cos(theta2);
num1    = (a2*cos(theta2) + a1)*x + (a2*sin(theta2))*y;     % from Eq.(5)
num2    = (a2*cos(theta2) + a1)*y - (a2*sin(theta2))*x;     % from Eq.(6)
theta1a = acos(num1/den)            % Eq.(7)

theta1a = 0.5236

theta1b = asin(num2/den)            % Eq.(8)

theta1b = 0.5236

In degree, the solution . Because of the periodic nature of the trigonometric functions in and , actually has an infinite number of solutions given by

and , where ℤ is the set of integers.

Answer 2

[Ayush Singh]

Hi Michael

From my understanding of the question you are trying to solve linear equations in a single variable.

You can refer to the article given below on how to solve it using "solve" function.

https://in.mathworks.com/help/symbolic/solve-a-system-of-linear-equations.html?searchHighlight=solving%20linear%20equations&s_tid=srchtitle_solving%2520linear%2520equations_1

[Walter Roberson]

If you restrict the variables to real values then solve will return empty.

[John D'Errico]

They are NOT linear equations, because theta1 is the unknown.

Answer 3

[Torsten]

Use the symbolic toolbox to solve the last equation for cos(theta1).

It's a polynomial equation of degree 4 which will have 4 roots:

x = a2*cos(theta1+theta2) + a1*cos(theta1);

x = a2*(cos(theta1)*cos(theta2)-sin(theta1)*sin(theta2))+a1*cos(theta1);

(x-a1*cos(theta1))^2 = a2*(cos(theta1)*cos(theta2)-sin(theta1)*sin(theta2))^2;

x^2-2*x*a1*cos(theta1)+a1^2*cos(theta1)^2 = a2*(cos(theta1)^2*cos(theta2)^2-...
2*cos(theta1)*cos(theta2)*sin(theta1)*sin(theta2) + sin(theta1)^2*sin(theta2)^2);

x^2-2*x*a1*cos(theta1)+a1^2*cos(theta1)^2-a2*(cos(theta1)^2*cos(theta2)^2+...
sin(theta1)^2*sin(theta2)^2) = -2*a2*cos(theta1)*cos(theta2)*sin(theta1)*sin(theta2);

(x^2-2*x*a1*cos(theta1)+a1^2*cos(theta1)^2-a2*(cos(theta1)^2*cos(theta2)^2+...
sin(theta1)^2*sin(theta2)^2))^2 = 4*a2^2*cos(theta1)^2*cos(theta2)^2*sin(theta1)^2*sin(theta2)^2;

(x^2-2*x*a1*cos(theta1)+a1^2*cos(theta1)^2-a2*(cos(theta1)^2*cos(theta2)^2+...
(1-cos(theta1)^2)*sin(theta2)^2))^2 = 4*a2^2*cos(theta1)^2*cos(theta2)^2*sin(theta2)^2*(1-cos(theta1)^2);

Answer 4

[John D'Errico]

If the only unknown is theta1, then what is y?

If you actually have a value for y, then you have two equations in one unknown variable, in which case there will generally be no exact solution for the pair of equations with one unknown. So I'll assume that is not the case.

If you do not know the value for y, then the second "equation" is completely irrelevant. You merely solve for theta1 from the first equation, since you know everything else.

syms x a2 a1 theta2 real
syms theta1 real
EQ = x == a2*cos(theta1+theta2) + a1*cos(theta1);

theta1sol = solve(EQ,theta1,'returnconditions',true)

theta1sol = struct with fields:
        theta1: [2×1 sym]
    parameters: k
    conditions: [2×1 sym]

theta1sol.conditions

ans = 

So, under some rather nasty set of conditions, that will depend on the constants in your problem, we have two solutions, found in:

theta1sol.theta1

ans = 

The problem is, there are some sets of parameters x, a1 and a2 that will create problems, where only complex solutions exist. When k==0, we get what would normally be called the primary solutions.

vpa(subs(theta1sol.theta1,[x,a1,a2,theta2],[0.25 .5 .5 pi/3]),10)

ans = 

We can ignore the tiny imaginary part there in the second solution. as just floating point trash. As you can see, there are infinitely many solutions, separated by multiples of 2*pi.

But for other sets of the parameters, only complex solutions exist. For example:

vpa(subs(theta1sol.theta1,[x,a1,a2,theta2],[1.5 .5 .5 pi/3]),10)

ans = 

Answer 5

[Walter Roberson]

syms a1 a2
syms theta1 theta2 x y
eqn = [x == a2*cos(theta1+theta2) + a1*cos(theta1)
       y == a2*sin(theta1+theta2) + a1*sin(theta1)]

eqn = 

sol1 = simplify(solve(eqn(1), theta1))

sol1 = 

sol2 = simplify(solve(eqn(2), theta1))

sol2 = 

d = simplify((sol1-sol2.'))

d = 

T1 = subs(d, [x,a1,a2,theta2],[0.25 .5 .5 pi/3])

T1 = 

arrayfun(@(E) simplify(solve(E,y)), T1, 'uniform', 0)

ans = 2×2 cell array
    {[-11^(1/2)/4]}    {0×1 sym}
    {[11^(1/2)/4 ]}    {0×1 sym}

cc1 = subs(eqn, [x,a1,a2,theta2,y], [0.25 .5 .5 pi/3, sqrt(sym(11))/4])

cc1 = 

cc1t1 = solve(cc1(1), theta1)

cc1t1 = 

cc1t2 = solve(cc1(2), theta1)

cc1t2 = 

T2 = subs(d, [x,a1,a2,theta2],[1.5 .5 .5 pi/3])

T2 = 

arrayfun(@(E) simplify(solve(E,y)), T2, 'uniform', 0)

ans = 2×2 cell array
    {[-(6^(1/2)*1i)/2]}    {0×1 sym}
    {[(6^(1/2)*1i)/2 ]}    {0×1 sym}

cc2 = subs(eqn, [x,a1,a2,theta2,y], [1.5 .5 .5 pi/3, sqrt(sym(6)*1i)/2])

cc2 = 

cc2t1 = simplify(solve(cc2(1), theta1), 'steps', 50)

cc2t1 = 

cc2t2 = simplify(solve(cc2(2), theta1), 'steps', 50)

cc2t2 = 

vpa(cc2t1 - cc2t2.')

ans = 

So... for some combinations of x, a1, a2, theta2, there exists a y such that theta1 is equal between the two equations... but for other combinations, although you can calculate y, it does not lead to self-consistent outcomes