How to solve a large 6-variable non-linear matrix determinate equation for all unique values.

I have a large jacobian matrix (to large to post) that is in terms of six variables: theta_1 thru theta_6, these variables are symbolic.

After taking the determinate, I need to solve the resulting equation for all possible unique values, this limits theta_n to be between [0,2pi) or [-pi,pi). There are additional maximum/minimums for each theta value but that is not critical right now.

Using the solve function has it running for an exceedingly long time and still not result, is there a faster way?

Code:

clear all
a = sym([0 174.0 0 0 0 0]');
d = sym([116.3 0 0 118.2 0 130]');
alpha = sym([pi/2 0 pi/2 -pi/2 pi/2 0]');
syms theta_1 theta_2 theta_3 theta_4 theta_5 theta_6;

T_0_1 = robot_transform(a(1), alpha(1), d(1), theta_1);

T_1_2 = robot_transform(a(2), alpha(2), d(2), theta_2);

T_2_3 = robot_transform(a(3), alpha(3), d(3), theta_3);

T_3_4 = robot_transform(a(4), alpha(4), d(4), theta_4);

T_4_5 = robot_transform(a(5), alpha(5), d(5), theta_5);

T_5_6 = robot_transform(a(6), alpha(6), d(6), theta_6);

T_0_6 = T_0_1 * T_1_2 * T_2_3 * T_3_4 * T_4_5 * T_5_6;

inputT = T_0_1;
Z1 = inputT(1:3,3);

inputT = inputT*T_1_2;
Z2 = inputT(1:3,3);

inputT = inputT*T_2_3;
Z3 = inputT(1:3,3);

inputT = inputT*T_3_4;
Z4 = inputT(1:3,3);

inputT = inputT*T_4_5;
Z5 = inputT(1:3,3);

inputT = inputT*T_5_6;
Z6 = inputT(1:3,3);

jacobianW = [Z1 Z2 Z3 Z4 Z5 Z6];
jacobianT = jacobian(T_0_6(1:3,4),[theta_1 theta_2 theta_3 theta_4 theta_5 theta_6]);

jacob = [jacobianT; jacobianW]

d = det(jacob);

disp('Solving determinate for singularities');
solution = solve(d==0, [theta_1 theta_2 theta_3 theta_4 theta_5 theta_6])

The function robot_transform

function T = robot_transform(a, alpha, d, theta)

T = [cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta);
sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta);
0 sin(alpha) cos(alpha) d;
0 0 0 1];

end

Answer 1

[John D'Errico]

This is an almost impossible problem to solve symbolically. Why? symbolic computations using determinants become INCREDIBLY complicated. Then hoping to solve for a symbolic solution will take an immense amount of time. But even if you wait forever, that still won't happen. Why not?

I looked at your result, jacob. A quick scan through it, and I never saw the unknown theta_6. So then, I had MATLAB look for me.

F = matlabFunction(jacob)
F =
function_handle with value:
@(theta_1,theta_2,theta_3,theta_4,theta_5) .....

Essentially, you have 6 equations, but NONE of them are a function of theta_6. Yes, you used it in the computations, but it got killed off somehow. I've not looked at the coefficients used or how Robot_transform uses them, but theta_6 never comes out the back end.

So in the end, you are asking solve to solve a problem with 6 equations, in 5 unknowns.

And that alone pretty much insures that solve will never find a solution. You might instead look at other approaches. For example, we can do this:

[L,U] = lu(jacob);

Surprisingly, that ran quite quickly. Now, we can compute the determinant of jacob as the product of the diagonal elements of the matrix U, as computed by the LU decomposition. If any of those diagonal elements is zero, then the matrix jacob will be singular. So you might decide to try a solution based on that, perhaps using a numerical optimization tool.

But hoping to solve this using symbolic tools is still impossible, at least not for some years to come.

[Connor LeClaire]

Thanks for the great answer!

This is a very interesting work-around, I never would have found it on my own.

I have added the following in lieu of det(jacob) and attempting to solving the determinate:

[L,U] = lu(jacob);
eqn = [U(1,1) U(2,2) U(3,3) U(4,4) U(5,5) U(6,6)];
disp('Solving determinate for singularities');
solution = solve(eqn(1)==0, [theta_1 theta_2 theta_3 theta_4 theta_5],'Real',true);

This gives me a set of 3 solutions across the five variables in a reasonably short amount of time (hurrah!). Is there a simple way to extract the data from the returned struct into something like a matrix? Perhaps having theta_n as the columns and the solutions as the rows?

[John D'Errico]

I'm actually slightly surprised solve managed to find that set of three solutions.

Nothing stops you from concatenating them into a matrix.

sol11 = vpa([solution.theta_1,solution.theta_2,solution.theta_3,solution.theta_4,solution.theta_5])
sol11 =
[0.37608965127468830761212367346143, 0, 0, 1.0, 1.0]
[                                 0, 0, 0,   0,   0]
[-2.7655030023151049308505197098181, 0, 0, 1.0, 1.0]

So each row is a solution. And there will surely be other solutions, because you could have tried solving for the (2,2) element to be zero. They may be more difficult to solve. Regardless, what you did seems to have killed off U(1,1), and that is really all you need. All three of them seem to work.

>> vpa(subs(eqn(1),[theta_1,theta_2,theta_3,theta_4,theta_5],sol11(1,:)))
ans =
-9.403954806578300063749892297778e-38

>> vpa(subs(eqn(1),[theta_1,theta_2,theta_3,theta_4,theta_5],sol11(2,:)))
ans =
0.0

>> vpa(subs(eqn(1),[theta_1,theta_2,theta_3,theta_4,theta_5],sol11(3,:)))
ans =
4.701977403289150031874946148889e-38

[Connor LeClaire]

Brilliant thank-you, this is exactly what I needed, just wasn't sure on how to go about it.

Many many many thanks.