robotics.m

这是一个能解决一系列机器人手运动学和动力学 方程的matlab符号工具箱。

function edit49_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit31 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called
global Iyz3
Iyz3 = str2double(get(hObject,'string'))

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc
    set(hObject,'BackgroundColor','white');
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end



function edit49_Callback(hObject, eventdata, handles)
% hObject    handle to edit31 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global Iyz3
Iyz3 = str2double(get(hObject,'string'))

% Hints: get(hObject,'String') returns contents of edit31 as text
%        str2double(get(hObject,'String')) returns contents of edit31 as a double


% --- Executes during object creation, after setting all properties.
function edit50_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit32 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called
global Izz3
Izz3 = str2double(get(hObject,'string'))

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc
    set(hObject,'BackgroundColor','white');
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end



function edit50_Callback(hObject, eventdata, handles)
% hObject    handle to edit32 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global Izz3
Izz3 = str2double(get(hObject,'string'))

% Hints: get(hObject,'String') returns contents of edit32 as text
%        str2double(get(hObject,'String')) returns contents of edit32 as a double






% --- Executes on button press in pushbutton1.
function pushbutton1_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
robotics2

%**************************************************************************
%**************************************************************************
%**************************************************************************
%**************************************************************************
%**************************************************************************

function robotics2
global vtau2 vtau3 vtau4
global link1 link2 link3
global center1 center2 center3
global mass1 mass2 mass3
global forcex forcey forcez
global torquex torquey torquez
global teta01 teta02 teta03
global gravity simtime
global Ixx1 Iyy1 Izz1 Ixy1 Ixz1 Iyz1
global Ixx2 Iyy2 Izz2 Ixy2 Ixz2 Iyz2
global Ixx3 Iyy3 Izz3 Ixy3 Ixz3 Iyz3
%clear all
%clc
%===== Definition of Symbolic Parameters =====
syms tet_d_1 tet_dd_1 tet_d_2 tet_dd_2       
syms tet_d_3 tet_dd_3 tet_d_4 tet_dd_4
syms g t1 t2 t3 t4 t4 t5 m1 m2 m3 m4
syms l1 l2 l3 l4 lc1 lc2 lc3 lc4
syms ixx1 ixy1 ixz1 iyy1 iyz1 izz1
syms ixx2 ixy2 ixz2 iyy2 iyz2 izz2
syms ixx3 ixy3 ixz3 iyy3 iyz3 izz3
syms ixx4 ixy4 ixz4 iyy4 iyz4 izz4
syms w0x w0y w0z
syms wd0x wd0y wd0z
syms vd0x vd0y vd0z
syms vdc0x vdc0y vdc0z
syms fx fy fz
syms nx ny nz
%*********************************************

w0=[w0x;w0y;w0z];
wd0=[wd0x;wd0y;wd0z];
vd0=[vd0x;vd0y;vd0z];
vdc0=[vdc0x;vdc0y;vdc0z];

nn=4;
%We have nn Coordinate System with Index 1,2,3,...n   
f(:,nn+1)=[fx;fy;fz];
n(:,nn+1)=[nx;ny;nz];

P(:,1)=[l1;0;0];   %Distance of C.S(1) From C.S(0)
P(:,2)=[l2;0;0];   %Distance of C.S(2) From C.S(1)
P(:,3)=[l3;0;0];   %Distance of C.S(3) From C.S(2)
P(:,4)=[l4;0;0];   %Distance of C.S(4) From C.S(3)

Pc(:,1)=[lc1;0;0]; %Distance of C.M(1) From C.S(1)
Pc(:,2)=[lc2;0;0];
Pc(:,3)=[lc3;0;0];
Pc(:,4)=[lc4;0;0];


w(:,1) = w0;
wd(:,1)= wd0;
vd(:,1) = vd0;
vdc(:,1) = vdc0;

m = [m1;m2;m3;m4];  % Mass Matrix

td(:,1)=[0;0;tet_d_1];    % Teta_dot_1
td(:,2)=[0;0;tet_d_2];    % Teta_dot_2
td(:,3)=[0;0;tet_d_3];    % Teta_dot_3
td(:,4)=[0;0;tet_d_4];    % Teta_dot_4
tdd(:,1)=[0;0;tet_dd_1];  % Teta_ddot_1
tdd(:,2)=[0;0;tet_dd_2];  % Teta_ddot_2
tdd(:,3)=[0;0;tet_dd_3];  % Teta_ddot_3
tdd(:,4)=[0;0;tet_dd_4];  % Teta_ddot_4
%
%
% Coordinates Relations
R(:,:,1) = [cos(t2)  sin(t2) 0;  
       -sin(t2) cos(t2) 0;
       0        0       1];%R12
R(:,:,2) = [cos(t3)  sin(t3) 0;  
       -sin(t3) cos(t3) 0;
       0        0       1];%R23
R(:,:,3) = [cos(t4)  sin(t4) 0;  
       -sin(t4) cos(t4) 0;
       0        0       1];%R34
R(:,:,4) = [cos(t5)  sin(t5) 0;  
       -sin(t5) cos(t5) 0;
       0        0       1];%R45
   
RR(:,:,2) = [cos(t2) -sin(t2) 0;  
       sin(t2)  cos(t2) 0;
       0        0       1];%R21
RR(:,:,3) = [cos(t3) -sin(t3) 0; 
       sin(t3)  cos(t3) 0;
       0        0       1];%R32
RR(:,:,4) = [cos(t4) -sin(t4) 0; 
       sin(t4)  cos(t4) 0;
       0        0       1];%R43
RR(:,:,5) = [cos(t5) -sin(t5) 0; 
       sin(t5)  cos(t5) 0;
       0        0       1];%R54
%_______________________________
%   
% Moments of Inertia of Arms
% With respect to C.M.
Ic(:,:,1)=[ixx1,ixy1,ixz1;
     ixy1,iyy1,iyz1;
     ixz1,iyz1,izz1];
Ic(:,:,2)=[ixx2,ixy2,ixz2;
     ixy2,iyy2,iyz2;
     ixz2,iyz2,izz2];
Ic(:,:,3)=[ixx3,ixy3,ixz3;
     ixy3,iyy3,iyz3;
     ixz3,iyz3,izz3];
Ic(:,:,4)=[ixx4,ixy4,ixz4;
     ixy4,iyy4,iyz4;
     ixz4,iyz4,izz4];
%________________________________    
% ****Iterative Calculations*****
% Internal Calculations for 0-->3
for i=[1:1:nn-1]
    w(:,i+1)= R(:,:,i)*w(:,i) + td(:,i+1);
    wd(:,i+1)=R(:,:,i)*wd(:,i) + cross(R(:,:,i)*w(:,i),td(:,i+1)) + tdd(:,i+1);
    vd(:,i+1)=R(:,:,i)*(cross(wd(:,i),P(:,i)) + cross(w(:,i),cross(w(:,i),P(:,i))) + vd(:,i));
    vdc(:,i+1)=cross(wd(:,i+1),Pc(:,i+1)) + cross(w(:,i+1),cross(w(:,i+1),Pc(:,i+1))) + vd(:,i+1);
    F(:,i+1)=m(i+1)*vdc(:,i+1);
    N(:,i+1)=Ic(:,:,i+1)*wd(:,i+1) + cross(w(:,i+1),Ic(:,:,i+1)*w(:,i+1));
end
%
% Extenal Calculations for 4-->1
for i=[nn:-1:2]
    f(:,i)=RR(:,:,i+1)*f(:,i+1) + F(:,i);
    n(:,i)=N(:,i) + RR(:,:,i+1)*n(:,i+1) + cross(Pc(:,i),F(:,i)) + cross(P(:,i),RR(:,:,i+1)*f(:,i+1));
end
%
%______________End of Calculation______________*
%***********************************************
%***********************************************
%____________Parameters Evaluations____________*
%_______________________________________________
%
%***********************************************
%******** Robot Parameters Evaluation **********
%

%vtau2 =50;  % Active Torque of actuator on Arm 2
%vtau3 =-30;  % Active Torque of actuator on Arm 3
%vtau4 =-10;  % Active Torque of actuator on Arm 4
stime = simtime;  % Simulation Time
g=gravity;     % Gravity

%Initial Angular Position of Arms
t1=0;t2=teta01*pi/180;t3=teta02*pi/180;t4=teta03/180;t5=0;

w0x=0;w0y=0;w0z=0;        %Angular Velocity of C.S(1)
wd0x=0;wd0y=0;wd0z=0;     %Angular Acceleration of C.S(1)
vd0x=0;vd0y=g;vd0z=0;     %Linear Acceleration of C.S(1)
vdc0x=0;vdc0y=g;vdc0z=0;  %Acceleration of Center of Mass (1)

% Robot Arms Configurations
l1=0; l2=link1; l3=link2; l4=link3;   %Arms Lengths
lc1=0;lc2=center1;lc3=center2;lc4=center3;   %positions ofCenter of mass
m1=0;m2=mass1;m3=mass2;m4=mass3;        %Arms Mass

% Arms Moments of Inertias Calculated in Center of Mass
ixx1=0; ixy1=0; ixz1=0; iyy1=0; iyz1=0; izz1=0;
ixx2=Ixx1; ixy2=Ixy1; ixz2=Ixz1; iyy2=Iyy1; iyz2=Iyz1; izz2=Izz1;
ixx3=Ixx2; ixy3=Ixy2; ixz3=Ixz2; iyy3=Iyy2; iyz3=Iyz2; izz3=Izz2;
ixx4=Ixx3; ixy4=Ixy3; ixz4=Ixz3; iyy4=Iyy3; iyz4=Iyz3; izz4=Izz3;


% Force and Torques affected on End Effector
fx=forcex; fy=forcey; fz=forcez;  %Forces
nx=torquex; ny=torquey; nz=torquez;  %Torques

% Angular Velocities and Accelerations of Arms
tet_d_1=0; tet_dd_1=0; 
%tet_d_2=1; tet_dd_2=1;
%tet_d_3=2; tet_dd_3=2;
%tet_d_4=4; tet_dd_4=3;
%**********************************************************
%**********************************************************

tau4 = eval(n(3,2));
tau3 = eval(n(3,3));
tau2 = eval(n(3,4));


tau4=simple(tau4);
tau3=simple(tau3);
tau2=simple(tau2);

syms torque4 torque3 torque2;

tau4 = torque4-tau4;
tau3 = torque3-tau3;
tau2 = torque2-tau2;

res = solve(tau4,tau3,tau2,tet_dd_4,tet_dd_3,tet_dd_2);

tx2=char(simple(res.tet_dd_2));     % Tet_dd_2 = fcn(tau2,tau3,tau4,tet2,tet3,tet4,tet_d_2,tet_d3,tet_d4...)
tx3=char(simple(res.tet_dd_3));     % Tet_dd_3 = fcn(tau2,tau3,tau4,tet2,tet3,tet4,tet_d_2,tet_d3,tet_d4...)
tx4=char(simple(res.tet_dd_4));     % Tet_dd_4 = fcn(tau2,tau3,tau4,tet2,tet3,tet4,tet_d_2,tet_d3,tet_d4...)
%
%_______________________________________________________________________
%________ Function generator for Differential Equation Solver __________
%***********************************************************************
%
I = findstr(tx2,'tet_d_2');
if(~isempty(I))
    t44 = tx2(1:I(1)-1);
    for i = 2:length(I) t44 = [t44 'y(2)' tx2(I(i-1)+7:I(i)-1)]; end
    t44 = [t44 'y(2)' tx2(I(end)+7:end)];
else
    t44 = tx2;
end

I = findstr(t44,'tet_d_3'); 
if(~isempty(I))
    t45 = t44(1:I(1)-1);
    for i = 2:length(I) t45 = [t45 'y(4)' t44(I(i-1)+7:I(i)-1)]; end
    t45 = [t45 'y(4)' t44(I(end)+7:end)];
else
    t45 = t44;
end

I = findstr(t45,'tet_d_4'); 
if(~isempty(I))
    t46 = t45(1:I(1)-1);
    for i = 2:length(I) t46 = [t46 'y(6)' t45(I(i-1)+7:I(i)-1)]; end
    t46 = [t46 'y(6)' t45(I(end)+7:end)];
else
    t46 = t45;
end
text2=t46;
%********************************
I = findstr(tx3,'tet_d_2');
if(~isempty(I))
    t44 = tx3(1:I(1)-1);
    for i = 2:length(I) t44 = [t44 'y(2)' tx3(I(i-1)+7:I(i)-1)]; end
    t44 = [t44 'y(2)' tx3(I(end)+7:end)];
else
    t44 = tx3;
end

I = findstr(t44,'tet_d_3'); 
if(~isempty(I))
    t45 = t44(1:I(1)-1);
    for i = 2:length(I) t45 = [t45 'y(4)' t44(I(i-1)+7:I(i)-1)]; end
    t45 = [t45 'y(4)' t44(I(end)+7:end)];
else
    t45 = t44;
end

I = findstr(t45,'tet_d_4'); 
if(~isempty(I))
    t46 = t45(1:I(1)-1);
    for i = 2:length(I) t46 = [t46 'y(6)' t45(I(i-1)+7:I(i)-1)]; end
    t46 = [t46 'y(6)' t45(I(end)+7:end)];
else
    t46 = t45;
end
text3=t46;
%********************************
I = findstr(tx4,'tet_d_2');
if(~isempty(I))
    t44 = tx4(1:I(1)-1);
    for i = 2:length(I) t44 = [t44 'y(2)' tx4(I(i-1)+7:I(i)-1)]; end
    t44 = [t44 'y(2)' tx4(I(end)+7:end)];
else
    t44 = tx4;
end

I = findstr(t44,'tet_d_3'); 
if(~isempty(I))
    t45 = t44(1:I(1)-1);
    for i = 2:length(I) t45 = [t45 'y(4)' t44(I(i-1)+7:I(i)-1)]; end
    t45 = [t45 'y(4)' t44(I(end)+7:end)];
else
    t45 = t44;
end

I = findstr(t45,'tet_d_4'); 
if(~isempty(I))
    t46 = t45(1:I(1)-1);
    for i = 2:length(I) t46 = [t46 'y(6)' t45(I(i-1)+7:I(i)-1)]; end
    t46 = [t46 'y(6)' t45(I(end)+7:end)];
else
    t46 = t45;
end
text4=t46;
%
%_______________________________________________________________________
%________ Writing the equations in the teta2.m for simulation __________
%***********************************************************************
%
fid = fopen('simulation.m','w');

fprintf(fid,'function simulati