Файл: Проектирование реализации операций бизнес-процесса «Движение библиотечного фонда» (ДИАГРАММА ПОСЛЕДОВАТЕЛЬНОСТИ).pdf
Добавлен: 30.06.2023
Просмотров: 174
Скачиваний: 5
% Theta 5: 200 (-100 to 100) 0 Wrist Bend
% Theta 6: 532 (-266 to 266) 0 Wrist Swivel
t1_home = 90; % offset to define the "home" position as UP.
t2_home = -90;
t3_home = -90;
LD = 105; % Left, used to set the GUI.
HT = 18; % Height
BT = 156; % Bottom
%% GUI buttons for Theta 1. pos is: [left bottom width height]
t1_slider = uicontrol(K_p,'style','slider',...
'Max',160,'Min',-160,'Value',0,...
'SliderStep',[0.05 0.2],...
'callback',@t1_slider_button_press,...
'Position',[LD BT 120 HT]);
t1_min = uicontrol(K_p,'style','text',...
'String','-160',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t1_max = uicontrol(K_p,'style','text',...
'String','+160',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t1_text = uibutton(K_p,'style','text',... % Nice program Doug. Need this
'String','\theta_1',... % due to no TeX in uicontrols.
'Position',[LD-100 BT 20 HT]); % L, B, W, H
% t1_text = uicontrol(K_p,'style','text',... % when matlab fixes uicontrol
% 'String','t1',... % for TeX, then I can use this.
% 'Position',[LD-100 BT 20 HT]); % L, B, W, H
t1_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t1_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% GUI buttons for Theta 2.
BT = 126; % Bottom
t2_slider = uicontrol(K_p,'style','slider',...
'Max',115,'Min',-115,'Value',0,... % Mech. stop limits !
'SliderStep',[0.05 0.2],...
'callback',@t2_slider_button_press,...
'Position',[LD BT 120 HT]);
t2_min = uicontrol(K_p,'style','text',...
'String','-110',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t2_max = uicontrol(K_p,'style','text',...
'String','+110',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t2_text = uibutton(K_p,'style','text',...
'String','\theta_2',...
'Position',[LD-100 BT 20 HT]); % L, B, W, H
t2_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t2_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% GUI buttons for Theta 3.
BT = 96; % Bottom
t3_slider = uicontrol(K_p,'style','slider',...
'Max',135,'Min',-135,'Value',0,...
'SliderStep',[0.05 0.2],...
'callback',@t3_slider_button_press,...
'Position',[LD BT 120 HT]);
t3_min = uicontrol(K_p,'style','text',...
'String','-135',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t3_max = uicontrol(K_p,'style','text',...
'String','+135',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t3_text = uibutton(K_p,'style','text',...
'String','\theta_3',...
'Position',[LD-100 BT 20 HT]); % L, B, W, H
t3_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t3_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% GUI buttons for Theta 4.
BT = 66; % Bottom
t4_slider = uicontrol(K_p,'style','slider',...
'Max',266,'Min',-266,'Value',0,...
'SliderStep',[0.05 0.2],...
'callback',@t4_slider_button_press,...
'Position',[LD BT 120 HT]);
t4_min = uicontrol(K_p,'style','text',...
'String','-266',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t4_max = uicontrol(K_p,'style','text',...
'String','+266',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t4_text = uibutton(K_p,'style','text',...
'String','\theta_4',...
'Position',[LD-100 BT 20 HT]); % L, B, W, H
t4_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t4_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% GUI buttons for Theta 5.
BT = 36; % Bottom
t5_slider = uicontrol(K_p,'style','slider',...
'Max',100,'Min',-100,'Value',0,...
'SliderStep',[0.05 0.2],...
'callback',@t5_slider_button_press,...
'Position',[LD BT 120 HT]);
t5_min = uicontrol(K_p,'style','text',...
'String','-100',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t5_max = uicontrol(K_p,'style','text',...
'String','+100',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t5_text = uibutton(K_p,'style','text',...
'String','\theta_5',...
'Position',[LD-100 BT 20 HT]); % L, B, W, H
t5_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t5_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% GUI buttons for Theta 6.
BT = 6; % Bottom
t6_slider = uicontrol(K_p,'style','slider',...
'Max',266,'Min',-266,'Value',0,...
'SliderStep',[0.05 0.2],...
'callback',@t6_slider_button_press,...
'Position',[LD BT 120 HT]);
t6_min = uicontrol(K_p,'style','text',...
'String','-266',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t6_max = uicontrol(K_p,'style','text',...
'String','+266',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t6_text = uibutton(K_p,'style','text',...
'String','\theta_6',...
'Position',[LD-100 BT 20 HT]); % L, B, W, H
t6_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t6_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% Slider for Theta 1 motion.
%
function t1_slider_button_press(h,dummy)
slider_value = round(get(h,'Value'));
set(t1_edit,'string',slider_value);
T_Old = getappdata(0,'ThetaOld');
t2old = T_Old(2); t3old = T_Old(3); t4old = T_Old(4);
t5old = T_Old(5); t6old = T_Old(6);
pumaANI(slider_value+t1_home,t2old,t3old,t4old,t5old,t6old,10,'n')
end
%
%% Slider for Theta 2 motion.
%
function t2_slider_button_press(h,dummy)
slider_value = round(get(h,'Value'));
set(t2_edit,'string',slider_value);
T_Old = getappdata(0,'ThetaOld');
t1old = T_Old(1); t3old = T_Old(3); t4old = T_Old(4);
t5old = T_Old(5); t6old = T_Old(6);
pumaANI(t1old,slider_value+t2_home,t3old,t4old,t5old,t6old,10,'n')
end
%
%% Slider for Theta 3 motion.
function t3_slider_button_press(h,dummy)
slider_value = round(get(h,'Value'));
set(t3_edit,'string',slider_value);
T_Old = getappdata(0,'ThetaOld');
t1old = T_Old(1); t2old = T_Old(2); t4old = T_Old(4);
t5old = T_Old(5); t6old = T_Old(6);
pumaANI(t1old,t2old,slider_value+t3_home,t4old,t5old,t6old,10,'n')
end
%
%% Slider for Theta 4 motion.
function t4_slider_button_press(h,dummy)
slider_value = round(get(h,'Value'));
set(t4_edit,'string',slider_value);
T_Old = getappdata(0,'ThetaOld');
t1old = T_Old(1); t2old = T_Old(2); t3old = T_Old(3);
t5old = T_Old(5); t6old = T_Old(6);
pumaANI(t1old,t2old,t3old,slider_value,t5old,t6old,10,'n')
end
%
%% Slider for Theta 5 motion.
function t5_slider_button_press(h,dummy)
slider_value = round(get(h,'Value'));
set(t5_edit,'string',slider_value);
T_Old = getappdata(0,'ThetaOld');
t1old = T_Old(1); t2old = T_Old(2); t3old = T_Old(3);
t4old = T_Old(4); t6old = T_Old(6);
pumaANI(t1old,t2old,t3old,t4old,slider_value,t6old,10,'n')
end
%
%% Slider for Theta 6 motion.
function t6_slider_button_press(h,dummy)
slider_value = round(get(h,'Value'));
set(t6_edit,'string',slider_value);
T_Old = getappdata(0,'ThetaOld');
t1old = T_Old(1); t2old = T_Old(2); t3old = T_Old(3);
t4old = T_Old(4); t5old = T_Old(5);
pumaANI(t1old,t2old,t3old,t4old,t5old,slider_value,10,'n')
end
%
%% Edit box for Theta 1 motion.
%
function t1_edit_button_press(h,dummy)
user_entry = check_edit(h,-160,160,0,t1_edit);
set(t1_slider,'Value',user_entry); % slider = text box.
T_Old = getappdata(0,'ThetaOld'); % Current pose
%
t2old = T_Old(2); t3old = T_Old(3); t4old = T_Old(4);
t5old = T_Old(5); t6old = T_Old(6);
%
pumaANI(user_entry+t1_home,t2old,t3old,t4old,t5old,t6old,10,'n')
end
%
%% Edit box for Theta 2 motion.
%
function t2_edit_button_press(h,dummy)
user_entry = check_edit(h,-110,110,0,t2_edit);
set(t2_slider,'Value',user_entry); % slider = text box.
T_Old = getappdata(0,'ThetaOld'); % Current pose
%
t1old = T_Old(1); t3old = T_Old(3); t4old = T_Old(4);
t5old = T_Old(5); t6old = T_Old(6);
%
pumaANI(t1old,user_entry+t2_home,t3old,t4old,t5old,t6old,10,'n')
end
%% Edit box for Theta 3 motion.
%
function t3_edit_button_press(h,dummy)
user_entry = check_edit(h,-135,135,0,t3_edit);
set(t3_slider,'Value',user_entry); % slider = text box.
T_Old = getappdata(0,'ThetaOld'); % Current pose
%
t1old = T_Old(1); t2old = T_Old(2); t4old = T_Old(4);
t5old = T_Old(5); t6old = T_Old(6);
%
pumaANI(t1old,t2old,user_entry+t3_home,t4old,t5old,t6old,10,'n')
end
%%
%% Edit box for Theta 4 motion.
%
function t4_edit_button_press(h,dummy)
user_entry = check_edit(h,-266,266,0,t4_edit);
set(t4_slider,'Value',user_entry); % slider = text box.
T_Old = getappdata(0,'ThetaOld'); % Current pose
%
t1old = T_Old(1); t2old = T_Old(2); t3old = T_Old(3);
t5old = T_Old(5); t6old = T_Old(6);
%
pumaANI(t1old,t2old,t3old,user_entry,t5old,t6old,10,'n')
end
%% Edit box for Theta 5 motion.
%
function t5_edit_button_press(h,dummy)
user_entry = check_edit(h,-100,100,0,t5_edit);
set(t5_slider,'Value',user_entry); % slider = text box.
T_Old = getappdata(0,'ThetaOld'); % Current pose
%
t1old = T_Old(1); t2old = T_Old(2); t3old = T_Old(3);
t4old = T_Old(4); t6old = T_Old(6);
%
pumaANI(t1old,t2old,t3old,t4old,user_entry,t6old,10,'n')
end
%%
%% Edit box for Theta 6 motion.
%
function t6_edit_button_press(h,dummy)
user_entry = check_edit(h,-266,266,0,t6_edit);
set(t6_slider,'Value',user_entry); % slider = text box.
T_Old = getappdata(0,'ThetaOld'); % Current pose
%
t1old = T_Old(1); t2old = T_Old(2); t3old = T_Old(3);
t4old = T_Old(4); t5old = T_Old(5);
%
pumaANI(t1old,t2old,t3old,t4old,t5old,user_entry,10,'n')
end
%%
function user_entry = check_edit(h,min_v,max_v,default,h_edit)
% This function will check the value typed in the text input box
% against min and max values, and correct errors.
%
% h: handle of gui
% min_v min value to check
% max_v max value to check
% default is the default value if user enters non number
% h_edit is the edit value to update.
%
user_entry = str2double(get(h,'string'));
if isnan(user_entry)
errordlg(['You must enter a numeric value, defaulting to ',num2str(default),'.'],'Bad Input','modal')
set(h_edit,'string',default);
user_entry = default;
end
%
if user_entry < min_v
errordlg(['Minimum limit is ',num2str(min_v),' degrees, using ',num2str(min_v),'.'],'Bad Input','modal')
user_entry = min_v;
set(h_edit,'string',user_entry);
end
if user_entry > max_v
errordlg(['Maximum limit is ',num2str(max_v),' degrees, using ',num2str(max_v),'.'],'Bad Input','modal')
user_entry = max_v;
set(h_edit,'string',user_entry);
end
end
%
%% Demo button's callback
function demo_button_press(h,dummy)
%
% disp('pushed demo bottom');
% R = 500;
% x = 1000;
n = 2; % demo ani steps
num = 30; % home to start, and end to home ani steps
% j = 1;
% M = 1000;
for t = 0:.1:7*pi
Px = 30*t*cos(t);
Py = 1200-300*t*(t)/(50*pi);
Pz = 30*t*sin(t);
[theta1,theta2,theta3,theta4,theta5,theta6] = PumaIK(Px,Py,Pz);
if t==0 %move to start of demo
pumaANI(theta1,theta2,theta3-180,0,0,0,num,'n')
end
% Theta 4, 5 & 6 are zero due to plotting at wrist origen.
pumaANI(theta1,theta2,theta3-180,0,0,0,n,'y')
set(t1_edit,'string',round(theta1)); % Update slider and text.
set(t1_slider,'Value',round(theta1));
set(t2_edit,'string',round(theta2));
set(t2_slider,'Value',round(theta2));
set(t3_edit,'string',round(theta3-180));
set(t3_slider,'Value',round(theta3-180));
end
gohome
% pumaANI(90,-90,-90,0,0,0,num,'n')
end
%
%
%%
function home_button_press(h,dummy)
%disp('pushed home bottom');
gohome
end
%
%%
function clr_trail_button_press(h,dummy)
%disp('pushed clear trail bottom');
handles = getappdata(0,'patch_h'); %
Tr = handles(9);
%
setappdata(0,'xtrail',0); % used for trail tracking.
setappdata(0,'ytrail',0); % used for trail tracking.
setappdata(0,'ztrail',0); % used for trail tracking.
%
set(Tr,'xdata',0,'ydata',0,'zdata',0);
end
%
%
function rnd_demo_button_press(h, dummy)
%disp('pushed random demo bottom');
% a = 10; b = 50; x = a + (b-a) * rand(5)
% Angle Range Default Name
% Theta 1: 320 (-160 to 160) 90 Waist Joint
% Theta 2: 220 (-110 to 110) -90 Shoulder Joint
% Theta 3: 270 (-135 to 135) -90 Elbow Joint
% Theta 4: 532 (-266 to 266) 0 Wrist Roll
% Theta 5: 200 (-100 to 100) 0 Wrist Bend
% Theta 6: 532 (-266 to 266) 0 Wrist Swival
t1_home = 90; % offsets to define the "home" postition as UP.
t2_home = -90;
t3_home = -90;
theta1 = -160 + 320*rand(1); % offset for home
theta2 = -110 + 220*rand(1); % in the UP pos.
theta3 = -135 + 270*rand(1);
theta4 = -266 + 532*rand(1);
theta5 = -100 + 200*rand(1);
theta6 = -266 + 532*rand(1);
n = 50;
pumaANI(theta1+t1_home,theta2+t2_home,theta3+t3_home,theta4,theta5,theta6,n,'y')
set(t1_edit,'string',round(theta1)); % Update slider and text.
set(t1_slider,'Value',round(theta1));
set(t2_edit,'string',round(theta2));
set(t2_slider,'Value',round(theta2));
set(t3_edit,'string',round(theta3));
set(t3_slider,'Value',round(theta3));
set(t4_edit,'string',round(theta4));
set(t4_slider,'Value',round(theta4));
set(t5_edit,'string',round(theta5));
set(t5_slider,'Value',round(theta5));
set(t6_edit,'string',round(theta6));
set(t6_slider,'Value',round(theta6));
end
%%
%Here are the functions used for this robot example:
%
%%
% When called this function will simply initialize a plot of the Puma 762
% robot by plotting it in it's home orientation and setting the current
% angles accordingly.
function gohome()
pumaANI(90,-90,-90,0,0,0,20,'n') % show it animate home
%PumaPOS(90,-90,-90,0,0,0) %drive it home, no animate.
set(t1_edit,'string',0);
set(t1_slider,'Value',0); %At the home position, so all
set(t2_edit,'string',0); %sliders and input boxes = 0.
set(t2_slider,'Value',0);
set(t3_edit,'string',0);
set(t3_slider,'Value',0);
set(t4_edit,'string',0);
set(t4_slider,'Value',0);
set(t5_edit,'string',0);
set(t5_slider,'Value',0);
set(t6_edit,'string',0);
set(t6_slider,'Value',0);
setappdata(0,'ThetaOld',[90,-90,-90,0,0,0]);
end
%%
% This function will load the 3D CAD data.
%
function loaddata
% Loads all the link data from file linksdata.mat.
% This data comes from a Pro/E 3D CAD model and was made with cad2matdemo.m
% from the file exchange. All link data manually stored in linksdata.mat
[linkdata]=load('linksdata.mat','s1','s2', 's3','s4','s5','s6','s7','A1');
%Place the robot link 'data' in a storage area
setappdata(0,'Link1_data',linkdata.s1);
setappdata(0,'Link2_data',linkdata.s2);
setappdata(0,'Link3_data',linkdata.s3);
setappdata(0,'Link4_data',linkdata.s4);
setappdata(0,'Link5_data',linkdata.s5);
setappdata(0,'Link6_data',linkdata.s6);
setappdata(0,'Link7_data',linkdata.s7);
setappdata(0,'Area_data',linkdata.A1);
end
%
%%
% Use forward kinematics to place the robot in a specified configuration.
%
function PumaPOS(theta1,theta2,theta3,theta4,theta5,theta6)
s1 = getappdata(0,'Link1_data');
s2 = getappdata(0,'Link2_data');
s3 = getappdata(0,'Link3_data');
s4 = getappdata(0,'Link4_data');
s5 = getappdata(0,'Link5_data');
s6 = getappdata(0,'Link6_data');
s7 = getappdata(0,'Link7_data');
A1 = getappdata(0,'Area_data');
%
a2 = 650;
a3 = 0;
d3 = 190;
d4 = 600;
Px = 5000;
Py = 5000;
Pz = 5000;
t1 = theta1;
t2 = theta2;
t3 = theta3 %-180;
t4 = theta4;
t5 = theta5;
t6 = theta6;
%
% Forward Kinematics
T_01 = tmat(0, 0, 0, t1);
T_12 = tmat(-90, 0, 0, t2);
T_23 = tmat(0, a2, d3, t3);
T_34 = tmat(-90, a3, d4, t4);
T_45 = tmat(90, 0, 0, t5);
T_56 = tmat(-90, 0, 0, t6);
%T_01 = T_01;
T_02 = T_01*T_12;
T_03 = T_02*T_23;
T_04 = T_03*T_34;
T_05 = T_04*T_45;
T_06 = T_05*T_56;
%
Link1 = s1.V1;
Link2 = (T_01*s2.V2')';
Link3 = (T_02*s3.V3')';
Link4 = (T_03*s4.V4')';
Link5 = (T_04*s5.V5')';
Link6 = (T_05*s6.V6')';
Link7 = (T_06*s7.V7')';
handles = getappdata(0,'patch_h'); %
L1 = handles(1);
L2 = handles(2);
L3 = handles(3);
L4 = handles(4);
L5 = handles(5);
L6 = handles(6);
L7 = handles(7);
%
set(L1,'vertices',Link1(:,1:3),'facec', [0.717,0.116,0.123]);
set(L1, 'EdgeColor','none');
set(L2,'vertices',Link2(:,1:3),'facec', [0.216,1,.583]);
set(L2, 'EdgeColor','none');
set(L3,'vertices',Link3(:,1:3),'facec', [0.306,0.733,1]);
set(L3, 'EdgeColor','none');
set(L4,'vertices',Link4(:,1:3),'facec', [1,0.542,0.493]);
set(L4, 'EdgeColor','none');
set(L5,'vertices',Link5(:,1:3),'facec', [0.216,1,.583]);
set(L5, 'EdgeColor','none');
set(L6,'vertices',Link6(:,1:3),'facec', [1,1,0.255]);
set(L6, 'EdgeColor','none');
set(L7,'vertices',Link7(:,1:3),'facec', [0.306,0.733,1]);
set(L7, 'EdgeColor','none');
end
%%
% This function computes the Inverse Kinematics for the Puma 762 robot
% given X,Y,Z coordinates for a point in the workspace. Note: The IK are
% computed for the origin of Coordinate systems 4,5 & 6.
function [theta1,theta2,theta3,theta4,theta5,theta6] = PumaIK(Px,Py,Pz)
theta4 = 0;
theta5 = 0;
theta6 = 0;
sign1 = 1;
sign3 = 1;
nogo = 0;
noplot = 0;
% Because the sqrt term in theta1 & theta3 can be + or - we run through
% all possible combinations (i = 4) and take the first combination that
% satisfies the joint angle constraints.
while nogo == 0;
for i = 1:1:4
if i == 1
sign1 = 1;
sign3 = 1;
elseif i == 2
sign1 = 1;
sign3 = -1;
elseif i == 3
sign1 = -1;
sign3 = 1;
else
sign1 = -1;
sign3 = -1;
end
a2 = 650;
a3 = 0;
d3 = 190;
d4 = 600;
rho = sqrt(Px^2+Py^2);
phi = atan2(Py,Px);
K = (Px^2+Py^2+Pz^2-a2^2-a3^2-d3^2-d4^2)/(2*a2);
c4 = cos(theta4);
s4 = sin(theta4);
c5 = cos(theta5);
s5 = sin(theta5);
c6 = cos(theta6);
s6 = sin(theta6);
theta1 = (atan2(Py,Px)-atan2(d3,sign1*sqrt(Px^2+Py^2-d3^2)));
c1 = cos(theta1);
s1 = sin(theta1);
theta3 = (atan2(a3,d4)-atan2(K,sign3*sqrt(a3^2+d4^2-K^2)));
c3 = cos(theta3);
s3 = sin(theta3);
t23 = atan2((-a3-a2*c3)*Pz-(c1*Px+s1*Py)*(d4-a2*s3),(a2*s3-d4)*Pz+(a3+a2*c3)*(c1*Px+s1*Py));
theta2 = (t23 - theta3);
c2 = cos(theta2);
s2 = sin(theta2);
s23 = ((-a3-a2*c3)*Pz+(c1*Px+s1*Py)*(a2*s3-d4))/(Pz^2+(c1*Px+s1*Py)^2);
c23 = ((a2*s3-d4)*Pz+(a3+a2*c3)*(c1*Px+s1*Py))/(Pz^2+(c1*Px+s1*Py)^2);
r13 = -c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
r23 = -s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
r33 = s23*c4*s5 - c23*c5;
theta4 = atan2(-r13*s1+r23*c1,-r13*c1*c23-r23*s1*c23+r33*s23);
r11 = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)+s1*(s4*c5*c6+c4*s6);
r21 = s1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-c1*(s4*c5*c6+c4*s6);
r31 = -s23*(c4*c5*c6-s4*s6)-c23*s5*c6;
s5 = -(r13*(c1*c23*c4+s1*s4)+r23*(s1*c23*c4-c1*s4)-r33*(s23*c4));
c5 = r13*(-c1*s23)+r23*(-s1*s23)+r33*(-c23);
theta5 = atan2(s5,c5);
s6 = -r11*(c1*c23*s4-s1*c4)-r21*(s1*c23*s4+c1*c4)+r31*(s23*s4);
c6 = r11*((c1*c23*c4+s1*s4)*c5-c1*s23*s5)+r21*((s1*c23*c4-c1*s4)*c5-s1*s23*s5)-r31*(s23*c4*c5+c23*s5);
theta6 = atan2(s6,c6);
theta1 = theta1*180/pi;
theta2 = theta2*180/pi;
theta3 = theta3*180/pi;
theta4 = theta4*180/pi;
theta5 = theta5*180/pi;
theta6 = theta6*180/pi;
if theta2>=160 && theta2<=180
theta2 = -theta2;
end
if theta1<=160 && theta1>=-160 && (theta2<=20 && theta2>=-200) && theta3<=45 && theta3>=-225 && theta4<=266 && theta4>=-266 && theta5<=100 && theta5>=-100 && theta6<=266 && theta6>=-266
nogo = 1;
theta3 = theta3+180;
break