2017-01-16 18:06:15 +00:00
|
|
|
%*** CHAPTER 3: RIGID-BODY MOTIONS ***
|
|
|
|
|
|
|
|
|
|
function AdT = Adjoint(T)
|
|
|
|
|
% Takes T a transformation matrix SE3.
|
|
|
|
|
% Returns the corresponding 6x6 adjoint representation [AdT].
|
|
|
|
|
% Example Input:
|
|
|
|
|
%{
|
2018-04-30 15:42:43 +00:00
|
|
|
clear; clc;
|
2017-01-16 18:06:15 +00:00
|
|
|
T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
|
|
|
|
|
AdT = Adjoint(T)
|
|
|
|
|
%}
|
|
|
|
|
% Output:
|
|
|
|
|
% AdT =
|
|
|
|
|
% 1 0 0 0 0 0
|
|
|
|
|
% 0 0 -1 0 0 0
|
|
|
|
|
% 0 1 0 0 0 0
|
|
|
|
|
% 0 0 3 1 0 0
|
|
|
|
|
% 3 0 0 0 0 -1
|
|
|
|
|
% 0 0 0 0 1 0
|
|
|
|
|
|
|
|
|
|
[R, p] = TransToRp(T);
|
|
|
|
|
AdT = [R, zeros(3); VecToso3(p) * R, R];
|
|
|
|
|
end
|
