clc; clear all;

NNN=2;
tic
for i=1:10000
seta1=-pi/NNN + (pi/NNN - (-pi/NNN)) * rand(1);
seta2=-pi/NNN + (pi/NNN - (-pi/NNN)) * rand(1);
seta3=-pi/NNN + (pi/NNN - (-pi/NNN)) * rand(1);
seta4=-pi/NNN + (pi/NNN - (-pi/NNN)) * rand(1);
seta5=-pi/NNN + (pi/NNN - (-pi/NNN)) * rand(1);
seta6=-pi/NNN + (pi/NNN - (-pi/NNN)) * rand(1);

initial_theta_1=[seta1,seta2,seta3,seta4,seta5,seta6];
initial_theta_2=[seta1,seta2,seta3,seta4,seta5,seta6]*0.99;

FK_value=robot_forward_kinematics(initial_theta_2);
i
Deri_theta=robot_inverse_kinematics_all_solutions(initial_theta_1,FK_value);
% robot_inverse_kinematics_all_solutions(initial_theta_1,FK_value);
error=Deri_theta-initial_theta_2;
   if any(abs(error) > 0.1)  % 任意关节误差绝对值>0.1则跳出
        fprintf('第%d次运行中检测到误差超标！\n', i);
        fprintf('误差向量：\n');
        disp(error);
        break_flag = true;
        break;  % 跳出循环
   end
end
t=toc
fprintf('代码运行时间：%.4f 秒\n', t);

% seta1_offset=0.00;
% seta2_offset=pi/2;
% seta3_offset=pi/2;
% seta4_offset=0;
% seta5_offset=pi/2;
% seta6_offset=0;
% % 
% FK_value(3,3)
% -sin(seta2_offset+seta2)*sin(seta3_offset+seta3+seta4_offset+seta4+seta5_offset+seta5)
% 
% asin(-FK_value(3,3)/sin(seta2_offset+seta2))-seta3_offset-seta3-seta4_offset-seta4-seta5_offset