calibration of sth

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, -1000]';
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 = [500,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 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 = -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);

HTfM_DRF01_DRF01toDRF02 =   HTfM_W_WtoDRF00*HTfM_W_DRF01toDRF02*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01%  Take the right 
DRF02 = HTfM_DRF01_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);

EAZYX_W_DRF00toW = inv(HTfM_W_WtoDRF00);

HTfM_DRF00_DRF02toDRF03 =   HTfM_W_WtoDRF00*HTfM_W_DRF02toDRF03*inv(HTfM_W_WtoDRF00)*HTfM_DRF01_DRF01toDRF02%  Take the right 
DRF03 = HTfM_DRF00_DRF02toDRF03*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')


%%  Transformation （ Four ）
%  Dynamic reference coordinate system DRF0X, The initial state and DRF02 The coordinate systems are perfectly coincident 
%（1）DRF0X Around DRF00 Of Z Shaft rotation 
for i=-pi/2:pi/10:-pi/10

    %%  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;     % 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%  Take the right 
    DRF02 = HTfM_DRF01_DRF01toDRF02*W;


    for j=0:pi/8:2*pi
        %%  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;        % 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%  Take the right 
        DRF0X = HTfM_DRF00_DRF02toDRF0X*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')
    end




end