How to convert existing symbolic equations for use in fsolve

2 views (last 30 days)
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

Answers (0)

Categories

Find more on Get Started with Optimization Toolbox in Help Center and File Exchange

Products


Release

R2021a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!