Solving non-linear equations symbolicaly

22 Ansichten (letzte 30 Tage)
Caio Vinicius do Carmo
Caio Vinicius do Carmo am 15 Jun. 2021
Hi,
I'm trying find the solution for the equations system of 12 equation and 6 variables for a robotic manipulator 6-DOF.
However, the matlab stays in busy and don't find the solution
%MATLAB 2015
%symbolic variables
syms theta1 theta2 theta3 theta4 theta5 theta6 a1 a2 d3 d4 d5
syms nx ny nz ox oy oz ax ay az px py pz py
%%Forward Kinematic ---------
% Transformation matrices
T1 = [cos(theta1) -sin(theta1) 0 0;sin(theta1) cos(theta1) 0 0 ; 0 0 1 0; 0 0 0 1]*[1 0 0 a1;0 1 0 0;0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 0 -sin(deg2rad(90)) 0; 0 sin(deg2rad(90)) 0 0 ; 0 0 0 1]
T2 = [cos(theta2) -sin(theta2) 0 0;sin(theta2) cos(theta2) 0 0 ; 0 0 1 0; 0 0 0 1]*[1 0 0 a2;0 1 0 0;0 0 1 0; 0 0 0 1]
T3 = [cos(theta3) -sin(theta3) 0 0;sin(theta3) cos(theta3) 0 0 ; 0 0 1 0; 0 0 0 1]*[1 0 0 d3;0 1 0 0;0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 0 -sin(deg2rad(-90)) 0; 0 sin(deg2rad(-90)) 0 0 ; 0 0 0 1]
T4 = [cos(theta4) -sin(theta4) 0 0;sin(theta4) cos(theta4) 0 0 ; 0 0 1 0; 0 0 0 1]*[1 0 0 0;0 1 0 0;0 0 1 -d4; 0 0 0 1]*[1 0 0 0; 0 0 -sin(deg2rad(-90)) 0; 0 sin(deg2rad(-90)) 0 0 ; 0 0 0 1]
T5 = [cos(theta5) -sin(theta5) 0 0;sin(theta5) cos(theta5) 0 0 ; 0 0 1 0; 0 0 0 1]*[1 0 0 d5;0 1 0 0;0 0 1 0; 0 0 0 1]*[1 0 0 0; 0 0 -sin(deg2rad(-90)) 0; 0 sin(deg2rad(-90)) 0 0 ; 0 0 0 1]
T6 = [cos(theta6) -sin(theta6) 0 0;sin(theta6) cos(theta6) 0 0 ; 0 0 1 0; 0 0 0 1]
T06 = T1*T2*T3*T4*T5*T6;
RHS = T06;
%initial reference matrix
T = [nx ox ax px;ny oy ay py;nz oz az pz;0 0 0 1]
%% Inverse Kinematics
%T06 == T
eq1 = T06(1,1)==nx;
eq2 = T06(1,2)==ox;
eq3 = T06(1,3)==ax;
eq4 = T06(1,4)==px;
eq5 = T06(2,1)==ny;
eq6 = T06(2,2)==oy;
eq7 = T06(2,3)==ay;
eq8 = T06(2,4)==py;
eq9 = T06(3,1)==nz;
eq10 = T06(3,2)==oz;
eq11 = T06(3,3)==az;
eq12 = T06(3,4)==pz;
eqns = [eq1 eq2 eq3 eq4 eq5 eq6 eq7 eq8 eq9 eq10 eq11 eq12];
[solt1,solt2,solt3,solt4,solt5,solt6] = solve(eqns,[theta1 theta2 theta3 theta4 theta5 theta6])

Antworten (0)

Kategorien

Mehr zu Formula Manipulation and Simplification finden Sie in Help Center und File Exchange

Produkte


Version

R2015a

Community Treasure Hunt

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

Start Hunting!

Translated by