41 lines
1.5 KiB
Mathematica
41 lines
1.5 KiB
Mathematica
|
function [Rpr,Rcur] = group(Ppr,Pcur,lam,zi)
|
||
|
|
% Regroup inertial parameters
|
||
|
|
% Ppr - parameters for link i-1
|
||
|
|
% Pcur - parameters for link i
|
||
|
|
% lam - lambda matrix for link i
|
||
|
|
% zi - axis of rotation for lin i
|
||
|
|
% Return:
|
||
|
|
% Rpr - regrouped parameters for link i-1
|
||
|
|
% Rcur - regrouped parameters for link i
|
||
|
|
|
||
|
|
Rcur = Pcur;
|
||
|
|
% --------------------------------------------------------------------
|
||
|
|
% mass
|
||
|
|
% --------------------------------------------------------------------
|
||
|
|
Rpr = Ppr + Pcur(10)*lam(10,:)';
|
||
|
|
Rcur(10) = 0;
|
||
|
|
% --------------------------------------------------------------------
|
||
|
|
% mass center and inertia
|
||
|
|
% --------------------------------------------------------------------
|
||
|
|
if zi(3) == 1 % Z rotation, add mZ, combine XX and YY
|
||
|
|
sum = simplify(lam(1,:) + lam(4,:));
|
||
|
|
Rpr = Rpr + sum' * Pcur(4) + lam(9,:)' * Pcur(9);
|
||
|
|
Rcur(1) = Pcur(1) - Pcur(4);
|
||
|
|
Rcur(4) = 0;
|
||
|
|
Rcur(9) = 0;
|
||
|
|
elseif zi(2) == 1 % Y rotation, add mY, combine XX and ZZ
|
||
|
|
sum = simplify(lam(1,:) + lam(6,:));
|
||
|
|
Rpr = Rpr + sum' * Pcur(6) + lam(8,:)' * Pcur(8);
|
||
|
|
Rcur(1) = Pcur(1) - Pcur(6);
|
||
|
|
Rcur(6) = 0;
|
||
|
|
Rcur(8) = 0;
|
||
|
|
elseif zi(1) == 1 % X rotation, add mZ, combine YY and ZZ
|
||
|
|
sum = simplify(lam(4,:) + lam(6,:));
|
||
|
|
Rpr = Rpr + sum' * Pcur(6) + lam(7,:)' * Pcur(7);
|
||
|
|
Rcur(4) = Pcur(4) - Pcur(6);
|
||
|
|
Rcur(6) = 0;
|
||
|
|
Rcur(7) = 0;
|
||
|
|
else
|
||
|
|
disp("Wrong axis definition");
|
||
|
|
end
|
||
|
|
end
|