TWO-LINK PLANAR ROBOT ARM

DINÁMICA INVERSA
PRESENTADO POR: JHON FREDDY RODRIGUEZ LEÓN
PRESENTADO A: DR. ALEJANDRO ALFREDO LOZANO GUZMÁN
UNIDAD DE APRENDIZAJE CINEMÁTICA Y DINÁMICA DE MAQUINARIA
INSTITUTO POLITÉCNICO NACIONAL
11 DE NOVIEMBRE DEL 2016

Datos: L1=L2= 1m; m1=m1= 1Kg;

g=9.81m/seg2;

Condiciones iniciales:
t=0seg;
q1(0)= π/18 rad; q˙1(0)=0 rad/s;
q2(0)= π/6 rad; q˙2(0)=0 rad/s;
q1 f =π/6 rad q2f =π/3 rad.
 INTERFACE:

function fout = deriv(f, gx)

% deriv differentiates f with respect to g=g(t)
% the variable g=g(t) is a function of time
syms t x dx
lg = {diff(gx, t), gx};
lx = {dx, x};
f1 = subs(f, lg, lx);
f2 = diff(f1, x);
fout = subs(f2, lx, lg);

Código:

function varargout = jhoninversa(varargin)

% JHONINVERSA MATLAB code for jhoninversa.fig
% JHONINVERSA, by itself, creates a new JHONINVERSA or raises the
existing
% singleton*.
%
% H = JHONINVERSA returns the handle to a new JHONINVERSA or the
handle to
% the existing singleton*.
%
% JHONINVERSA('CALLBACK',hObject,eventData,handles,...) calls the
local
% function named CALLBACK in JHONINVERSA.M with the given input
arguments.
%
% JHONINVERSA('Property','Value',...) creates a new JHONINVERSA or
raises the
% existing singleton*. Starting from the left, property value pairs
are
% applied to the GUI before jhoninversa_OpeningFcn gets called. An
% unrecognized property name or invalid value makes property
application
% stop. All inputs are passed to jhoninversa_OpeningFcn via
varargin.
%
% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only
one
% instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help jhoninversa

% Last Modified by GUIDE v2.5 11-Nov-2016 18:04:05

% Begin initialization code - DO NOT EDIT

gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @jhoninversa_OpeningFcn, ...
'gui_OutputFcn', @jhoninversa_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before jhoninversa is made visible.

function jhoninversa_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% varargin command line arguments to jhoninversa (see VARARGIN)
% Choose default command line output for jhoninversa
handles.output = hObject;

% Update handles structure

guidata(hObject, handles);

% UIWAIT makes jhoninversa wait for user response (see UIRESUME)

% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.
function varargout = jhoninversa_OutputFcn(hObject, eventdata, handles)
% varargout cell array for returning output args (see VARARGOUT);
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure

varargout{1} = handles.output;

function edit1_Callback(hObject, eventdata, handles)

% hObject handle to edit1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit1 as text

% str2double(get(hObject,'String')) returns contents of edit1 as a
double
fa1= str2double(get(hObject,'String'));
save fa1;

% --- Executes during object creation, after setting all properties.

function edit1_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns
called

% Hint: edit controls usually have a white background on Windows.

% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function edit2_Callback(hObject, eventdata, handles)

% hObject handle to edit1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of edit1 as text
% str2double(get(hObject,'String')) returns contents of edit1 as a
double
fa2= str2double(get(hObject,'String'));
save fa2;

% --- Executes during object creation, after setting all properties.

function edit2_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns
called

% Hint: edit controls usually have a white background on Windows.

% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function edit3_Callback(hObject, eventdata, handles)

% hObject handle to edit3 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit3 as text

% str2double(get(hObject,'String')) returns contents of edit3 as a
double
fa3= str2double(get(hObject,'String'));
save fa3;

% --- Executes during object creation, after setting all properties.

function edit3_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit3 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns
called

% Hint: edit controls usually have a white background on Windows.

% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function edit4_Callback(hObject, eventdata, handles)

% hObject handle to edit4 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of edit4 as text
% str2double(get(hObject,'String')) returns contents of edit4 as a
double
fa4= str2double(get(hObject,'String'));
save fa4;

% --- Executes during object creation, after setting all properties.

function edit4_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit4 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns
called

% Hint: edit controls usually have a white background on Windows.

% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function edit5_Callback(hObject, eventdata, handles)

% hObject handle to edit5 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit5 as text

% str2double(get(hObject,'String')) returns contents of edit5 as a
double
fa5= str2double(get(hObject,'String'));
save fa5;

% --- Executes during object creation, after setting all properties.

function edit5_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit5 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns
called

% Hint: edit controls usually have a white background on Windows.

% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function edit6_Callback(hObject, eventdata, handles)

% hObject handle to edit6 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of edit6 as text
% str2double(get(hObject,'String')) returns contents of edit6 as a
double
fa6= str2double(get(hObject,'String'));
save fa6;

% --- Executes during object creation, after setting all properties.

function edit6_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit6 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns
called

% Hint: edit controls usually have a white background on Windows.

% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function edit7_Callback(hObject, eventdata, handles)

% hObject handle to edit6 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit6 as text

% str2double(get(hObject,'String')) returns contents of edit6 as a
double
fa7= str2double(get(hObject,'String'));
save fa7;

% --- Executes during object creation, after setting all properties.

function edit7_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit6 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns
called

% Hint: edit controls usually have a white background on Windows.

% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function edit8_Callback(hObject, eventdata, handles)

% hObject handle to edit5 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of edit5 as text
% str2double(get(hObject,'String')) returns contents of edit5 as a
double
fa8= str2double(get(hObject,'String'));
save fa8;

% --- Executes during object creation, after setting all properties.

function edit8_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit5 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns
called

% Hint: edit controls usually have a white background on Windows.

% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function edit9_Callback(hObject, eventdata, handles)

% hObject handle to edit9 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit9 as text

% str2double(get(hObject,'String')) returns contents of edit9 as a
double
fa9= str2double(get(hObject,'String'));
save fa9;

% --- Executes during object creation, after setting all properties.

function edit9_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit9 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns
called

% Hint: edit controls usually have a white background on Windows.

% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function edit10_Callback(hObject, eventdata, handles)

% hObject handle to edit9 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of edit9 as text
% str2double(get(hObject,'String')) returns contents of edit9 as a
double
fa10= str2double(get(hObject,'String'));
save fa10;

% --- Executes during object creation, after setting all properties.

function edit10_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit9 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns
called

% Hint: edit controls usually have a white background on Windows.

% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

% --- 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)

load fa1
load fa2
load fa3
load fa4
load fa5
load fa6
load fa7
load fa8
load fa9
load fa10

syms t L1 L2 m1 m2 m3 gx

q1 = sym('q1(t)');
q2 = sym('q2(t)');

LL1=fa1; %eslabon L1
LL2=fa2; %eslabon L2
MM1=fa3; %masa L1
MM2=fa4; %masa L2

%condiciones iniciales

q1s = fa5;%angulos q1 inicial

q2s = fa6; %angulos q2 inicial
q1f = fa7 ; %angulos q1 final
q2f = fa8; %angulos q2 final
GG=fa9; %gravedad
Tp=fa10; % tiempo final

c1 = cos(q1); s1 = sin(q1);
c2 = cos(q2); s2 = sin(q2);
xB = L1*c1; yB = L1*s1;
rB = [xB yB 0];
rC1 = rB/2; vC1 = diff(rC1,t);
xD = xB + L2*c2; yD = yB + L2*s2;
rD = [xD yD 0];
rC2 = (rB + rD)/2; vC2 = diff(rC2,t);
omega1 = [0 0 diff(q1,t)];
omega2 = [0 0 diff(q2,t)];
IA = m1*L1^2/3; IC2 = m2*L2^2/12;
T1 = IA*omega1*omega1.'/2;
T2 = m2*vC2*vC2.'/2 + IC2*omega2*omega2.'/2;
T = expand(T1 + T2);
%deriv(f,g(t)) differentiates f with respect to g(t)
Tdq1 = deriv(T, diff(q1,t));
Tdq2 = deriv(T, diff(q2,t));
Tt1 = diff(Tdq1, t); Tt2 = diff(Tdq2, t);
Tq1 = deriv(T, q1); Tq2 = deriv(T, q2);
LHS1 = Tt1 - Tq1; LHS2 = Tt2 - Tq2;
rC1_1 = deriv(rC1, q1); rC2_1 = deriv(rC2, q1);
rC1_2 = deriv(rC1, q2); rC2_2 = deriv(rC2, q2);
w1_1 = deriv(omega1, diff(q1,t));
w2_1 = deriv(omega2, diff(q1,t));
w1_2 = deriv(omega1, diff(q2,t));
w2_2 = deriv(omega2, diff(q2,t));
G1 = [0 -m1*gx 0]; G2 = [0 -m2*gx 0];

%torque of link 1 y link 2

syms T01z T12z
T01 = [0 0 T01z];

T12 = [0 0 T12z];
Q1=rC1_1*G1.'+w1_1*T01.'+w1_1*(-T12.')+rC2_1*G2.'+w2_1*T12.';
Q2=rC1_2*G1.'+w1_2*T01.'+w1_2*(-T12.')+rC2_2*G2.'+w2_2*T12.';
Lagrange1 = LHS1-Q1; Lagrange2 = LHS2-Q2;
data = {L1, L2, m1, m2, gx};
datn = {LL1 , LL2 , MM1 , MM2 , GG};
Lagr1 = subs(Lagrange1, data, datn);
Lagr2 = subs(Lagrange2, data, datn);
% solve for T01z T12z
sol = solve(Lagr1,Lagr2,'T01z, T12z');
T01zc = sol.T01z;
T12zc = sol.T12z;

% INVERSE DYNAMICS
q1n=q1s+(q1f-q1s)/Tp*(t-Tp/(2*pi)*sin(2*pi/Tp*t));
q2n=q2s+(q2f-q2s)/Tp*(t-Tp/(2*pi)*sin(2*pi/Tp*t));
dq1n = diff(q1n,t);
dq2n = diff(q2n,t);
ddq1n = diff(dq1n,t);
ddq2n = diff(dq2n,t);
ql={diff(q1,t,2),diff(q2,t,2),diff(q1,t),diff(q2,t),q1,q2};
qn={ddq1n, ddq2n, dq1n, dq2n, q1n, q2n};

T01zt = subs(T01zc, ql, qn);

T12zt = subs(T12zc, ql, qn);

% directa
time = 0:1:Tp;
q_1t = subs(q1n,'t',time);
q_2t = subs(q2n,'t',time);

%grafico directa angulos

% axes(handles.axes1);
% plot(time,q_1t*180/pi),xlabel('t (s)'),ylabel('q1
(deg)'),title('Grafico Directa q1'),grid;
%
% axes(handles.axes2);
% plot(time,q_2t*180/pi),xlabel('t (s)'),ylabel('q2
(deg)'),title('Grafico Directa q2 '),grid;

%grafico inversa

axes(handles.axes3);
ezplot(T01zt,[0,Tp]),xlabel('t (s)'), ylabel('T01z (N m)'),title('Grafico
Inversa T01z '), grid

axes(handles.axes4);
ezplot(T12zt,[0,Tp]),xlabel('t (s)'), ylabel('T12z (N m)'),title('Grafico
Inversa T12z'), grid

% --- Executes on button press in pushbutton2.

function pushbutton2_Callback(hObject, eventdata, handles)
% hObject handle to pushbutton2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

clear;
clear global;
res1=findobj(gcbf,'tag','edit1');
res2=findobj(gcbf,'tag','edit2');
res3=findobj(gcbf,'tag','edit3');
res4=findobj(gcbf,'tag','edit4');
res5=findobj(gcbf,'tag','edit5');
res6=findobj(gcbf,'tag','edit6');
res7=findobj(gcbf,'tag','edit7');
res8=findobj(gcbf,'tag','edit8');
res9=findobj(gcbf,'tag','edit9');
res10=findobj(gcbf,'tag','edit10');
limpia = '';
set (res1,'string' , limpia);
set (res2,'string' , limpia);
set (res3,'string' , limpia);
set (res4,'string' , limpia);
set (res5,'string' , limpia);
set (res6,'string' , limpia);
set (res7,'string' , limpia);
set (res8,'string' , limpia);
set (res9,'string' , limpia);
set (res10,'string' , limpia);