How to convert existing symbolic equations for use in fsolve

This question is an extension of a previous question here.

I have a series of six non-linear equations of five variables that all unique solutions must be found for. These equations come from the diagonals of the U matrix after decomposing the matrix of interest using the lu function and are solved for when equal to zero. Using the solve function has worked for the first equation however the others take a significant amount of time and perhaps cannot be solved using the solve function. I would like to use the fsolve function but do not know how to convert the existing symbolic equations into the proper form, nor specify the variables as per the examples in the documentation. If I am offbase in wanting to use the fsolve function if it will not work (or there is a better alternative) please let me know.

The function of interest is getSolution however the rest of the applicable code has been included if required.

Code:

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

%% Calculate Transforms
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];

[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');
sol1 = getSolution(eqn(1),[theta_1 theta_2 theta_3 theta_4 theta_5]);
disp('Sol 1 complete');

.
.
.

sol6 = getSolution(eqn(6),[theta_1 theta_2 theta_3 theta_4 theta_5]);
disp('Sol 6 complete');

function getSolution

function [M] = getSolution(eqn,vars)

solution = solve(eqn==0, vars ,'Real',true);
M = vpa(rad2deg([solution.theta_1,solution.theta_2,solution.theta_3,solution.theta_4,solution.theta_5]),4);

end

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

[Nipun]

Hi Connor,

I understand that you are trying to find unique solutions for a series of six nonlinear equations of five variables, derived from the diagonals of a U matrix after LU decomposition. You have successfully used the solve function for the first equation but are encountering performance issues with the others. You are considering using the fsolve function as an alternative but are unsure how to proceed with converting the symbolic equations into a numerical format suitable for fsolve, as well as specifying the variables correctly.

To use fsolve for your problem, you need to convert your symbolic equations into a format that fsolve can work with. fsolve is designed for numerical solutions of systems of nonlinear equations and requires the equations to be defined as a function that takes a vector of variables as input and returns a vector of equation values. Here is how you can modify your getSolution function to use fsolve:

function M = getSolution(eqn, vars)
    % Convert symbolic equations to a function handle
    % This requires the symbolic toolbox
    eqnFunc = matlabFunction(eqn, 'Vars', {vars});

    % Initial guess for the solution
    initialGuess = zeros(length(vars), 1);

    % Set options for fsolve to display output iteratively
    options = optimoptions('fsolve', 'Display', 'iter');

    % Solve the system of equations using fsolve
    [numericSolution,~,exitflag] = fsolve(eqnFunc, initialGuess, options);

    % Check if fsolve successfully found a solution
    if exitflag <= 0
        warning('fsolve did not converge to a solution.');
        M = [];
    else
        % Convert radians to degrees and format the output
        M = vpa(rad2deg(numericSolution), 4);
    end
end

In this revised getSolution function:

• eqn is expected to be your equation or system of equations.
• vars is a vector of your variables, which you previously defined as [theta_1 theta_2 theta_3 theta_4 theta_5].
• The function matlabFunction converts symbolic expressions to a function handle that fsolve can use.
• initialGuess is a vector of initial guesses for the variables. The quality of this guess can significantly affect fsolve's ability to find a solution.
• options is used to set fsolve options, such as displaying iterative output to monitor the solution process.
• numericSolution is the solution found by fsolve, with exitflag indicating whether the solution process was successful.

This approach should allow you to find numerical solutions to your system of equations using fsolve. Adjust the initialGuess based on any prior knowledge or constraints you have regarding the expected solutions to improve the chances of convergence.

Hope this helps.

Regards,

Nipun