MobRob_Lab2.m

quat_base_foot_print = [-0.5 0.5 -0.5 0.5];
rot_base_foot_print = quat2rotm(quat_base_foot_print)

quat_camera_frame = [0.99 -0.00031 0.0021 0.0040];
rot_camera_frame = quat2rotm(quat_camera_frame)


T_CR = [0  0 1 0.076; -1 0 0 0; 0 -1 0 0.103; 0 0 0 1] % Transformation of Camera w.r.t. Robot
T_AC = [1 -0.0081 0.0042 -0.0007; 0.0081 1 0.006 -0.094; -0.0042 -0.0006 1 0.9215; 0 0 0 1] % Transformation of AprilTag w.r.t. Camera
T_AG = [0 0 -1 0.12; -1 0 0 0; 0 1 0 0.2; 0 0 0 1] % Given in the question
T_GA = inv(T_AG) % Transformation of Goal w.r.t. AprilTag

% T_RC = inv(T_CR)
% T_CA = inv(T_AC)
% T_AR =  T_CR * T_AC

T_AR = [0 0 -1 1; -1 0 0 0; 0 1 0 0.2; 0 0 0 1] % Approximation for the sake of simplicity
T_RA = inv(T_AR) % Transformation of Robot w.r.t. AprilTag

% Tf = Final pose of the robot w.r.t. the april tag = T_GA
% T0 = Initial pose of the robot w.r.t the april tag = T_RA
% Tf = T0*expm(omega*t)
% omega = (1/t)*logm(T0_inverse * Tf)

t = 5;
omega = (1/t)*logm(T_AR * T_GA)