Code covered by the BSD License  

Highlights from
Kinematic/Dynamic Control of a Two Link Manipulator

image thumbnail

Kinematic/Dynamic Control of a Two Link Manipulator

by

 

29 Apr 2009 (Updated )

Kinematic and Dynamic models of a Two Link Manipulator undergo non-linear feedback linearization.

DYN_CL_B3(t,X)
%%%% DYNAMIC ANALYSIS : DYNAMIC ANALYSIS CLOSED LOOP B3

% Course: Robotic Manipulation and Mobility
% Advisor: Dr. V. Krovi
%
% Homework Number: 4
%
% Names: Sourish Chakravarty
% 	Hrishi Lalit Shah


function [dX]= DYN_CL_B3(t,X)

global l1 l2 lc1 lc2 j1 j2 m1 m2 g rx ry ell_an w Kp Kx Kp1 Kd1 A B % Given parameters
global itr Q1 Q2 Q3 Q4 Q5 Q6 Q7 Q8 t1 t2 t3 t4 t5 t6 t7 t8
global Tau1 Tau2 Tau3 Tau4

th1=X(1);
th2=X(2);
th1d=X(3);
th2d=X(4);


%% CREATING IMPORTANT MATRICES IN THE GOVERNING EQUATION

%% Acquiring desired theta values for points exactly on the ellipse%%%%%%%
THETA_DES=TH_DES_INFO(t,[th1,th2]);
th1_des=THETA_DES(1);
th2_des=THETA_DES(2);
th1d_des=THETA_DES(3);
th2d_des=THETA_DES(4);
th1dd_des=THETA_DES(5);
th2dd_des=THETA_DES(6);

s1_des=sin(th1_des);
s2_des=sin(th2_des);
c1_des=cos(th1_des);
c2_des=cos(th2_des);
c21_des=cos(th2_des-th1_des);
s21_des=sin(th2_des-th1_des);

%% Dynamic control matrices : Desired
Mdes = [ j1+m2*l1^2, m2*l1*lc2*c21_des;
      m2*l1*lc2*c21_des, j2+m2*lc2^2];
Vdes = [0, -m2*l1*lc2*s21_des;
    m2*l1*lc2*s21_des, 0];
Gdes = [m1*g*lc1*c1_des+m2*g*l1*c1_des;
    m2*g*lc2*c2_des];

%% Control Torque (based on ideal situation)
% Tau_des1= Mdes*[th1dd_des;th2dd_des] + Vdes*[th1d_des^2;th2d_des^2]+Gdes; %% Control Torque
Tau_des= Gdes+ Vdes*[th1d_des^2;th2d_des^2] + Mdes*( [th1dd_des;th2dd_des] + Kp1*([th1_des;th2_des]-[th1;th2]) + Kd1*([th1d_des;th2d_des]-[th1d;th2d])); %% Control Torque

%% Simulation Data
s1=sin(th1);
s2=sin(th2);
c1=cos(th1);
c2=cos(th2);
c21=cos(th2-th1);
s21=sin(th2-th1);

%%%%%% Dynamic control matrices
M = [ j1+m2*l1^2, m2*l1*lc2*c21;
      m2*l1*lc2*c21, j2+m2*lc2^2];
V = [0, -m2*l1*lc2*s21;
    m2*l1*lc2*s21, 0];
G = [m1*g*lc1*c1+m2*g*l1*c1;
    m2*g*lc2*c2];
%%%%%%%%%%%%%%%%%%%%%%%%%

%% Generating differential
[THDD]=inv(M)*(Tau_des- V*[th1d^2;th2d^2]-G); %%% Deriving angular acc
%% Simulation Update
dX(1,1)= th1d;
dX(2,1)= th2d;
dX(3,1)= THDD(1);
dX(4,1)= THDD(2);
%% Storage
itr=itr+1;
t7(itr)=t;
Q7(itr,1) = [th1];
Q7(itr,2) = [th2];
Q7(itr,3) = [th1d];
Q7(itr,4) = [th2d];
Q7(itr,5) = THDD(1);
Q7(itr,6) = THDD(2);
Tau4(itr,:)= Tau_des;

Contact us