generate pivot data 2

clc;
clear;
close all;


% rng(0);
% rand(1)*2*pi

%% W: world coordinate system
%  Size 4X4, Homogeneous matrix , Homogeneous coordinate system 
W = eye(4);
W(1:3,1:3)=W(1:3,1:3);%*200; %  multiply 200 To make the axis longer , It is convenient to display 


%%  Transformation （ zero ）
%  Dynamic reference coordinate system DRF00, The initial state completely coincides with the world coordinate system 
%（1） Around the W Of X Shaft rotation ,（2） Around the W Of Y Shaft rotation ,（3） Around the W Of Z Shaft rotation ,
%（4） be relative to W The origin of the coordinates , Along W Of XYZ Direction translation 
roll_W_WtoDRF00 = pi/2; % Z
yaw_W_WtoDRF00 = 0;     % Y
pitch_W_WtoDRF00 = 0;   % X
EAZYX_W_WtoDRF00 = [roll_W_WtoDRF00 yaw_W_WtoDRF00 pitch_W_WtoDRF00 ];
TlXYZ_W_WtoDRF00 = [100, -100, -1200]';
HTfM_W_WtoDRF00 = genHTfM_useEularAngle(EAZYX_W_WtoDRF00,TlXYZ_W_WtoDRF00);
DRF00 = HTfM_W_WtoDRF00*W;

%%  Transformation （ One ）
%  Dynamic reference coordinate system DRF01, The initial state and DRF00 The coordinate systems are perfectly coincident 
%（1） be relative to DRF00 The origin of the coordinates , Along DRF00 Of X Direction translation 
roll_W_DRF00toDRF01 = 0;    % Z
yaw_W_DRF00toDRF01 = 0;     % Y
pitch_W_DRF00toDRF01 = 0;   % X
EAZYX_W_DRF00toDRF01 = [roll_W_DRF00toDRF01 yaw_W_DRF00toDRF01 pitch_W_DRF00toDRF01 ];
TlXYZ_W_DRF00toDRF01 = [200,0,0]';
HTfM_W_DRF00toDRF01 = genHTfM_useEularAngle(EAZYX_W_DRF00toDRF01,TlXYZ_W_DRF00toDRF01);


%  Take the right （ First relative to W Transformation , And then do it W To DRF00 Transformation of ）
HTfM_DRF00_DRF00toDRF01 = HTfM_W_WtoDRF00*HTfM_W_DRF00toDRF01;
DRF01 = HTfM_DRF00_DRF00toDRF01*W;

%%  Transformation （ Two ）
%  Dynamic reference coordinate system DRF02TMP, The initial state and DRF01 The coordinate systems are perfectly coincident 
%（1）DRF02TMP Around DRF00 Of Y Shaft rotation , To get the final DRF02
roll_W_DRF01toDRF02 = 0;        % Z
yaw_W_DRF01toDRF02 = -pi/4;     % Y
pitch_W_DRF01toDRF02 = 0;       % X
EAZYX_W_DRF01toDRF02 = [roll_W_DRF01toDRF02 yaw_W_DRF01toDRF02 pitch_W_DRF01toDRF02];
TlXYZ_W_DRF01toDRF02 = [0,0,0]';
HTfM_W_DRF01toDRF02 = genHTfM_useEularAngle(EAZYX_W_DRF01toDRF02,TlXYZ_W_DRF01toDRF02);

EAZYX_W_DRF00toW = inv(HTfM_W_WtoDRF00);

% (1) We require a transformation matrix HTfM_DRF01_DRF01toDRF02, bring DRF02 = HTfM_DRF01_DRF01toDRF02*W
%  Because the transformation matrix is ultimately left multiplied coordinate system W, So the target coordinate system is needed here DRF02 Initial state of DRF02TMP00 And the coordinate system W coincidence 

% (2) Put the coordinate system DRF02TMP00 Transform to and coordinate system DRF01 Complete coincidence , obtain DRF02TMP01
% {DRF02TMP01 = HTfM_DRF00_DRF00toDRF01*W}

% (3) Put the coordinate system DRF00 And the coordinate system DRF02TMP01, As a whole , Change at the same time .
%  For the coordinate system DRF02TMP01 do , Put the coordinate system DRF00 Transform to a coordinate system W Transformation of , Left multiplication inv(HTfM_W_WtoDRF00), obtain DRF02TMP02
% {DRF02TMP02 = inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01*W}

% (4) Now the coordinate system W And the coordinate system DRF00 Complete coincidence , In the coordinate system W It's about   do DRF02TMP02 Around DRF00 Of Y Shaft rotation DRF01 To DRF02 Transformation of , obtain DRF02TMP03
%  Now the coordinate system W And the coordinate system DRF00 Complete coincidence , So relative to W To make a transformation is equivalent to DRF00 Make a change 
% {DRF02TMP03 = HTfM_W_DRF01toDRF02*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01*W}

% (5) Do the following from the coordinate system at this time W To DRF00 Transformation of , That's the process (3) The inverse transformation of , Transform matrix left multiply HTfM_W_WtoDRF00
% {DRF02 =  HTfM_W_WtoDRF00*HTfM_W_DRF01toDRF02*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01*W}

HTfM_DRF00_DRF01toDRF02 = HTfM_W_WtoDRF00*HTfM_W_DRF01toDRF02*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01;
DRF02 = HTfM_DRF00_DRF01toDRF02*W;

%%  Transformation （ 3、 ... and ）
%  Dynamic reference coordinate system DRF03, The initial state and DRF02 The coordinate systems are perfectly coincident 
%（1）DRF02 Around DRF00 Of Z Shaft rotation 
roll_W_DRF02toDRF03 = pi/2;        % Z
yaw_W_DRF02toDRF03 = 0;           % Y
pitch_W_DRF02toDRF03 = 0;        % X
EAZYX_W_DRF02toDRF03 = [roll_W_DRF02toDRF03 yaw_W_DRF02toDRF03 pitch_W_DRF02toDRF03];
TlXYZ_W_DRF02toDRF03 = [0,0,0]';
HTfM_W_DRF02toDRF03 = genHTfM_useEularAngle(EAZYX_W_DRF02toDRF03,TlXYZ_W_DRF02toDRF03);


% (1) We require a transformation matrix  HTfM_DRF00_DRF02toDRF03, bring DRF03 = HTfM_DRF00_DRF02toDRF03*W
%  Because the transformation matrix is ultimately left multiplied coordinate system W, So the target coordinate system is needed here DRF03 Initial state of DRF03TMP00 And the coordinate system W coincidence 

% (2) Put the coordinate system DRF03TMP00 Transform to and coordinate system DRF02 Complete coincidence , obtain DRF03TMP01
%  Borrow directly here   Transformation （ Two ） Result 
% {DRF03TMP01 = HTfM_DRF00_DRF01toDRF02*W}

% (3) Put the coordinate system DRF00 And the coordinate system DRF03TMP01, As a whole , Change at the same time .
%  For the coordinate system DRF03TMP01 do , Put the coordinate system DRF00 Transform to a coordinate system W Transformation of , Left multiplication inv(HTfM_W_WtoDRF00), obtain DRF03TMP02
% {DRF03TMP02 = inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF01toDRF02*W}

% (4) Now the coordinate system W And the coordinate system DRF00 Complete coincidence , In the coordinate system W It's about   do DRF03TMP02 Around DRF00 Of Z Shaft rotation DRF02 To DRF03 Transformation of , obtain DRF03TMP03
%  Now the coordinate system W And the coordinate system DRF00 Complete coincidence , So relative to W To make a transformation is equivalent to DRF00 Make a change 
% {DRF03TMP03 = HTfM_W_DRF02toDRF03*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF01toDRF02*W}

% (5) Do the following from the coordinate system at this time W To DRF00 Transformation of , That's the process (3) The inverse transformation of , Transform matrix left multiply HTfM_W_WtoDRF00
% {DRF03 = HTfM_W_WtoDRF00*HTfM_W_DRF02toDRF03*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF01toDRF02*W}

HTfM_DRF00_DRF02toDRF03 = HTfM_W_WtoDRF00*HTfM_W_DRF02toDRF03*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF01toDRF02;
DRF03 = HTfM_DRF00_DRF02toDRF03*W;


%%  Transformation （ Four ）
%  Dynamic reference coordinate system DRF04TMP, The initial state and DRF03 The coordinate systems are perfectly coincident 
%（1）DRF04TMP Around DRF03 Coordinate origin rotation , obtain DRF04
roll_W_DRF03toDRF04 = pi/3;        % Z
yaw_W_DRF03toDRF04 = pi/4;           % Y
pitch_W_DRF03toDRF04 = pi/2;        % X
EAZYX_W_DRF03toDRF04 = [roll_W_DRF03toDRF04 yaw_W_DRF03toDRF04 pitch_W_DRF03toDRF04];
TlXYZ_W_DRF03toDRF04 = [0,0,0]';
HTfM_W_DRF03toDRF04 = genHTfM_useEularAngle(EAZYX_W_DRF03toDRF04,TlXYZ_W_DRF03toDRF04);


% (1) We require a transformation matrix  HTfM_DRF00_DRF03toDRF04, bring DRF04 = HTfM_DRF00_DRF03toDRF04*W
%  Because the transformation matrix is ultimately left multiplied coordinate system W, So the target coordinate system is needed here DRF04 Initial state of DRF04TMP00 And the coordinate system W coincidence 

% (2) Put the coordinate system DRF04TMP00 Relative to the coordinate system W Do rotation transformation , obtain DRF04TMP01
% {DRF04TMP01 = HTfM_W_DRF03toDRF04*W}

% (3) For the coordinate system DRF04TMP01, Do from the coordinate system W To the coordinate system DRF03 Transformation of , obtain DRF04
% {DRF04 = HTfM_DRF00_DRF02toDRF03*HTfM_W_DRF03toDRF04*W}

DRF04 = HTfM_DRF00_DRF02toDRF03*HTfM_W_DRF03toDRF04*W;









%%
%  drawing 
axis_limit = 1200;
axis_limitA = [-axis_limit,axis_limit,-axis_limit,axis_limit,-1500,300];

figure(6)
title("figure(6)");
trplot(W,'frame',  'RW','color','r',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);

hold on;
plot3(W(1,4),W(2,4),W(3,4),'ro')
hold on;
trplot(DRF00,'frame',  'R00','color','g',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);




% hold on;
% trplot(DRF01, 'frame', 'R01', 'color', 'b', 'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
% hold on;
% plot3(DRF01(1,4),DRF01(2,4),DRF01(3,4),'bo')
%
% hold on;
% trplot(DRF02, 'frame', 'R02', 'color', 'k', 'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
% hold on;
% plot3(DRF02(1,4),DRF02(2,4),DRF02(3,4),'ko')
%
% hold on;
% trplot(DRF03, 'frame', 'RDRF03', 'color', 'm', 'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
% hold on;
% plot3(DRF03(1,4),DRF03(2,4),DRF03(3,4),'mo')
%
%
% hold on;
% trplot(DRF04, 'frame', 'RDRF04', 'color', 'c', 'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
% hold on;
% plot3(DRF04(1,4),DRF04(2,4),DRF04(3,4),'co')



%%  Transformation （ 5、 ... and ）
%  Dynamic reference coordinate system DRF0X, The initial state and DRF02 The coordinate systems are perfectly coincident 
%（1）DRF0X Around DRF00 Of Z Shaft rotation 

fid=fopen(['simulate_pivot.txt'], 'w');		%  Write file path 

for i=0 : -0.01 : -0.5-(-0.05)
    %  Transformation （ Two ）
    %  Dynamic reference coordinate system DRF02, The initial state and DRF01 The coordinate systems are perfectly coincident 
    %（1）DRF02 Around DRF00 Of Y Shaft rotation 
    roll_W_DRF01toDRF02 = 0;       % Z
    yaw_W_DRF01toDRF02 = i*pi;     % Y
    pitch_W_DRF01toDRF02 = 0;      % X
    EAZYX_W_DRF01toDRF02 = [roll_W_DRF01toDRF02 yaw_W_DRF01toDRF02 pitch_W_DRF01toDRF02];
    TlXYZ_W_DRF01toDRF02 = [0,0,0]';
    HTfM_W_DRF01toDRF02 = genHTfM_useEularAngle(EAZYX_W_DRF01toDRF02,TlXYZ_W_DRF01toDRF02);

    EAZYX_W_DRF00toW = inv(HTfM_W_WtoDRF00);

    HTfM_DRF01_DRF01toDRF02 =   HTfM_W_WtoDRF00*HTfM_W_DRF01toDRF02*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01;
    DRF02 = HTfM_DRF01_DRF01toDRF02*W;


    for j=0 : 0.02 : 2-0.02
        %  Transformation （ 3、 ... and ）
        %  Dynamic reference coordinate system DRF03, The initial state and DRF02 The coordinate systems are perfectly coincident 
        %（1）DRF02 Around DRF00 Of Z Shaft rotation 

        roll_W_DRF02toDRF0X = j*pi;        % Z
        yaw_W_DRF02toDRF0X = 0;         % Y
        pitch_W_DRF02toDRF0X = 0;       % X
        EAZYX_W_DRF02toDRF0X = [roll_W_DRF02toDRF0X yaw_W_DRF02toDRF0X pitch_W_DRF02toDRF0X];
        TlXYZ_W_DRF02toDRF0X = [0,0,0]';
        HTfM_W_DRF02toDRF0X = genHTfM_useEularAngle(EAZYX_W_DRF02toDRF0X,TlXYZ_W_DRF02toDRF0X);

        EAZYX_W_DRF00toW = inv(HTfM_W_WtoDRF00);

        HTfM_DRF00_DRF02toDRF0X =   HTfM_W_WtoDRF00*HTfM_W_DRF02toDRF0X*inv(HTfM_W_WtoDRF00)*HTfM_DRF01_DRF01toDRF02;
        DRF0X = HTfM_DRF00_DRF02toDRF0X*W;



        for k=0 : 1: 2
            %  Transformation （ Four ）
            %  Dynamic reference coordinate system DRF04TMP, The initial state and DRF03 The coordinate systems are perfectly coincident 
            %（1）DRF04TMP Around DRF03 Coordinate origin rotation , obtain DRF04
            roll_W_DRF03toDRF04 = k*rand()*pi;          % Z
            yaw_W_DRF03toDRF04 = k*rand()*pi;           % Y
            pitch_W_DRF03toDRF04 = k*rand()*pi;         % X
            EAZYX_W_DRF03toDRF04 = [roll_W_DRF03toDRF04 yaw_W_DRF03toDRF04 pitch_W_DRF03toDRF04];
            TlXYZ_W_DRF03toDRF04 = [0,0,0]';
            HTfM_W_DRF03toDRF04 = genHTfM_useEularAngle(EAZYX_W_DRF03toDRF04,TlXYZ_W_DRF03toDRF04);
            DRF0X = HTfM_DRF00_DRF02toDRF0X*HTfM_W_DRF03toDRF04*W;

            %             hold on;
            %             trplot(DRF0X, 'frame', 'RDRF0X', 'color', 'm', 'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
            %             hold on;
            %             plot3(DRF0X(1,4),DRF0X(2,4),DRF0X(3,4),'mo');

            %  Find its coordinate system DRF0X Next point , In the coordinate system W Coordinates and quaternions under 
            %  Its quaternion and coordinates DRF0X The quaternions of the coordinate origin are the same 
            DRF0X_TlXYZ_marker = [5, 20, 10]';

            R = DRF0X(1:3,1:3);
            T = DRF0X(1:3,4);

            W_TlXYZ_marker = R*DRF0X_TlXYZ_marker + T;

            W_quaternion_marker = rotm2quat(R);

            %  write in txt
            for i=1:length(W_quaternion_marker)
                fprintf(fid,'%.4f ',W_quaternion_marker(i));		%  Press line to output 
            end



            for i=1:length(W_TlXYZ_marker)
                fprintf(fid,'%.4f ',W_TlXYZ_marker(i));		%  Press line to output 

            end

            for i=1:length(W_TlXYZ_marker)
                fprintf(fid, '%.4f ',W_TlXYZ_marker(i));		%  Press line to output 

            end

            for i=1:length(W_TlXYZ_marker)
                fprintf(fid, '%.4f ',W_TlXYZ_marker(i));		%  Press line to output 

            end

            for i=1:length(W_TlXYZ_marker)
                if i==3
                    fprintf(fid,'%.4f ',W_TlXYZ_marker(i));		%  Press line to output 
                else
                    fprintf(fid,'%.4f ',W_TlXYZ_marker(i));		%  Press line to output 

                end
            end

            fprintf(fid,  '\r\n');


        end

    end
end

fclose(fid);
a = 0

function HTfM= genHTfM_useEularAngle(eularAngle_zyx,translation)
% HTfM homogeneous transform matrix
 
rotationMatrix=eul2rotm(eularAngle_zyx,'ZYX');
HTfM = eye(4,4);
HTfM(1:3,1:3) = rotationMatrix;
HTfM(1:3,4) = translation;
 
end