add SpatialInertia

2023-10-31 00:54:53 +08:00 · 2023-10-31 00:54:53 +08:00 · 10097c8db4
parent 8967270c39
commit 10097c8db4
1 changed files with 10 additions and 0 deletions
--- a/packages/MATLAB/mr/SpatialInertia.m
+++ 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