% Chapter 8 section 3 Demos

% Transforming wrenches from {B} to {A}
atb = [1;2;3];       % Set translation (expressed in {A})
aTb = transl(atb);   % Set {B} relative to {A}
aRb = aTb(1:3,1:3);  % Get the rotation matrix
bJa = tr2jac(aTb);   % Get the velocity transform Jacobian
frcb = [1;1;1];      % Set the force in {B} (expressed in {B})
momb = [1;1;1]*0.7;  % Set the moment in {B} (expressed in {B})
gb = [frcb; momb];   % Set the wrench in {B} (expressed in {B})

ga = bJa' * gb;      % Transform to {A} and express result in {A}

% Check
frca = aRb * frcb;   % Get force expressed in {A}
moma = aRb * momb + cross(atb, frca);   % Get moment expressed in {A}
g_error = ga - [frca; moma]

% Compare to spatial velocity with Principle of Virtual Work.
% The virtual work is the same in any frame.  
%    a^g_a' * a^nu_a = b^g_b' * b^nu_b                     (1)
% Substitute:   b^J_a * a^nu_a   in for    b^nu_b 
%    b^g_b' * b^J_a * a^nu_a                               (2)
% Now we also see that  b^g_b' * b^J_a = a^g_a.
% i.e., Section 8.3.1 is correct.
vela = [rand(3,1)];    % Set velocity of Aorg expressed in {A}
omegaa = [rand(3,1)];  % Set ang. vel. of A expresses in {A}
nua = [vela; omegaa];  % Set spatial velocity of Aorg in {A}
vwa = ga' * nua;
nub = bJa * nua;
vwb = gb' * nub;
vw_error = vwb - vwa