Optimization

351Appendix B

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% MATLAB code robotics_optimized_traj.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%

% Generates optimized trajectory for the robotics arm problem

function [] = generate_optimized_traj(x)

d1 = 66.04;

d3 = 14.91;

d4 = 43.31;

a2 = 43.18;

a3 = 2.03;

for i = 1:100

t = -pi + (i-1)*0.063;

theta1 = x(i);

theta2 = x(i+100);

theta3 = x(i+200);

c1 = cos(theta1);

c2 = cos(theta2);

s1 = sin(theta1);

s2 = sin(theta2);

c23 = cos(theta2+theta3);

s23 = sin(theta2+theta3);

f1(i) = c1*(a2*c2 + a3*c23 - d4*s23) - d3*s1;

f2(i) = s1*(a2*c2 + a3*c23 - d4*s23) + d3*c1;

f3(i) = d1 - a2*s2 - a3*s23 -d4*c23;

end

plot3(f1,f2,f3,'r*')

end

%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Chapter 4

Code Name Details

simplex.m Simplex method

initial_cost.m Computes cost coefcients for the nonbasic variables

phase1.m Phase I of the simplex method

remove_variable.m Removes user specied column from the nonbasic set

phase1_without_initialization.m Phase I of the simplex method without initializing A

matrix and b vector.

dual.m Dual simplex method

interior.m Afne scaling method

352 Appendix B

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% MATLAB code simplex.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%

% The matrix A and b corresponds to equation Ax=b

% c -> vector of cost coefficients

% basic_set -> set of basic variables

% nonbasic_set -> setof nonbasic variables

% B -> matrix containing basic variable columns of A

% N -> matrix containing nonbasic variable columns of A

% xb -> basic variables

% y -> simplex multipliers

% cb -> cost coefficients of basic variables

% cn -> cost coefficients of nonbasic variables

%

clear all

clc

format rational

format compact

A = [3 1 1 0 0;

1 2 0 1 0;

1 0 0 0 1];

b = [10;8;3];

c = [-6;-7;0;0;0];

basic_set = [3 4 5];

nonbasic_set = [1 2];

for i = 1:length(basic_set)

B(:,i) = A(:,basic_set(i));

cb(i) = c(basic_set(i));

end

for i = 1:length(nonbasic_set)

N(:,i) = A(:,nonbasic_set(i));

cn(i) = c(nonbasic_set(i));

end

cn_cap = cn;

cb_ini = cb;

b_cap = b;

zz1 = 0;

fprintf('\n ________________________________________\n')

basic_set

nonbasic_set

Initial_Table = [B N b_cap]

Cost =[cb cn_cap -zz1]

for i = 1:3

[minvalue entering_basic_variable] = min(cn_cap);

ente ring_column = inv(B)*A(:,nonbasic_set(entering_basic_

variable));

ratios = b_cap'./entering_column';

[min_ratio leaving_basic_variable] = min(ratios);

while min_ratio<0

353Appendix B

ratios(leaving_basic_variable) = inf;

[min_ratio leaving_basic_variable] = min(ratios);

end

temp_basic_set = basic_set;

temp_nonbasic_set = nonbasic_set;

temp_cb = cb;

temp_cn = cn;

basi c_set(leaving_basic_variable) = temp_nonbasic_

set(entering_basic_variable);

nonb asic_set(entering_basic_variable) = temp_basic_

set(leaving_basic_variable);

cb(leaving_basic_variable) = temp_cn(entering_basic_variable);

cn(entering_basic_variable) = temp_cb(leaving_basic_variable);

aa(nonbasic_set) = cn;

cn = aa(sort(nonbasic_set));

nonbasic_set = sort(nonbasic_set);

for ii = 1:length(basic_set)

B(:,ii) = A(:,basic_set(ii));

end

for ii = 1:length(nonbasic_set)

N(:,ii) = A(:,nonbasic_set(ii));

end

xb = inv(B)*b;

y = cb*inv(B);

cn_cap = cn-y*N;

b_cap = xb;

zz = zz1+cb*xb;

fprintf('\n ________________________________________\n')

basic_set

nonbasic_set

Table = [eye(length(B)) inv(B)*N b_cap]

Cost = [cb_ini cn_cap -zz]

if cn_cap >= 0

break;

end

end

fprintf('\n ------SOLUTION------\n')

basic_set

xb

zz

%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% MATLAB code initial_cost.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%

cb = [0 1 1];

cn = [0 0 0];

N = [3 2 0;

354 Appendix B

2 -4 -1;

3 4 0];

B = [0 1 0;0 0 1;1 0 0];

y = cb*inv(B);

cn_cap = cn-y*N

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% MATLAB code phase1.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%

% The matrix A and b corresponds to Ax=b

% c -> vector of cost coefficients

% basic_set -> set of basic variables

% nonbasic_set -> setof nonbasic variables

% B -> matrix containing basic variable columns of A

% N -> matrix containing nonbasic variable columns of A

% xb -> basic variables

% y -> simplex multipliers

% cb -> cost coefficients of basic variables

% cn -> cost coefficients of nonbasic variables

%

clear all

clc

format rational

format compact

A = [3 2 0 0 1 0;

2 -4 -1 0 0 1;

3 4 0 1 0 0];

b = [10;3;16];

c = [-5;2;1;0;0;0];

basic_set = [5 6 4];

nonbasic_set = [1 2 3];

for i = 1:length(basic_set)

B(:,i) = A(:,basic_set(i));

cb(i) = c(basic_set(i));

end

for i = 1:length(nonbasic_set)

N(:,i) = A(:,nonbasic_set(i));

cn(i) = c(nonbasic_set(i));

end

cn_cap = cn;

cb_ini = cb;

b_cap = b;

zz1 = 91/8;

fprintf('\n ________________________________________\n')

basic_set

nonbasic_set

Initial_Table = [B N b_cap]

Cost = [cb cn_cap -zz1]

355Appendix B

for i = 1:1

[minvalue entering_basic_variable] = min(cn_cap);

ente ring_column = inv(B)*A(:,nonbasic_set(entering_basic_

variable));

ratios = b_cap'./entering_column';

[min_ratio leaving_basic_variable] = min(ratios);

while min_ratio<0

ratios(leaving_basic_variable) = inf;

[min_ratio leaving_basic_variable] = min(ratios);

end

temp_basic_set = basic_set;

temp_nonbasic_set = nonbasic_set;

temp_cb = cb;

temp_cn = cn;

basic _set(leaving_basic_variable) = temp_nonbasic_

set(entering_basic_variable);

nonba sic_set(entering_basic_variable) = temp_basic_

set(leaving_basic_variable);

cb(leaving_basic_variable) = temp_cn(entering_basic_variable);

cn(entering_basic_variable) = temp_cb(leaving_basic_variable);

aa(nonbasic_set) = cn;

cn = aa(sort(nonbasic_set));

nonbasic_set = sort(nonbasic_set);

for ii = 1:length(basic_set)

B(:,ii) = A(:,basic_set(ii));

end

for ii = 1:length(nonbasic_set)

N(:,ii) = A(:,nonbasic_set(ii));

end

xb = inv(B)*b;

y = cb*inv(B);

cn_cap = cn-y*N;

b_cap = xb;

zz = zz1+cb*xb;

fprintf('\n ________________________________________\n')

basic_set

nonbasic_set

Table = [eye(length(B)) inv(B)*N b_cap]

Cost = [cb_ini cn_cap -zz]

end

%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% MATLAB code remove_variable.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%

% This program removes user specified column from

% the nonbasic set

%