Solving non-linear equations symbolicaly

조회 수: 18 (최근 30일)
Caio Vinicius do Carmo
Caio Vinicius do Carmo 2021년 6월 15일
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개)

카테고리

Help CenterFile Exchange에서 Formula Manipulation and Simplification에 대해 자세히 알아보기

제품


릴리스

R2015a

Community Treasure Hunt

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

Start Hunting!

Translated by