diff --git a/packages/MATLAB/mr/SpatialInertia.m b/packages/MATLAB/mr/SpatialInertia.m new file mode 100644 index 0000000..5a004e2 --- /dev/null +++ b/packages/MATLAB/mr/SpatialInertia.m @@ -0,0 +1,10 @@ +function Glist = SpatialInertia(G) +% *** CHAPTER x: DYNAMICS OF OPEN CHAINS *** +% Takes G: A list of inertia, +% Returns Glist: Spatial inertia matrices Gi of the links + +n = size(G,1); +Glist = zeros(6,6,n); +for i = 1:n + Glist(:,:,i) = diag(G(i,1:6)); +end