Solving non-linear equations symbolicaly
22 Ansichten (letzte 30 Tage)
Ältere Kommentare anzeigen
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])
0 Kommentare
Antworten (0)
Siehe auch
Kategorien
Mehr zu Formula Manipulation and Simplification finden Sie in Help Center und File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!