From 10097c8db40bf3845cdf9a3031c7f7026c0501ee Mon Sep 17 00:00:00 2001
From: cosmic_power <cosmic_power@163.com>
Date: Tue, 31 Oct 2023 00:54:53 +0800
Subject: [PATCH] add SpatialInertia

---
 packages/MATLAB/mr/SpatialInertia.m | 10 ++++++++++
 1 file changed, 10 insertions(+)
 create mode 100644 packages/MATLAB/mr/SpatialInertia.m

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