add performance optimization for sparse inertia matrices

Author: baggepinnen

Project.toml

@@ -13,6 +13,7 @@ MeshIO = "7269a6da-0436-5bbc-96c2-40638cbb6118"
 ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
 ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
+SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 
 [weakdeps]
@@ -31,6 +32,7 @@ MeshIO = "0.4"
 ModelingToolkit = "9"
 ModelingToolkitStandardLibrary = "2.7.2"
 Rotations = "1.4"
+SparseArrays = "1"
 StaticArrays = "1"
 julia = "1"
 

src/Multibody.jl

@@ -7,6 +7,7 @@ using ModelingToolkit
 using JuliaSimCompiler
 import ModelingToolkitStandardLibrary.Mechanical.Rotational
 import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational
+using SparseArrays
 using StaticArrays
 export Rotational, Translational
 

src/components.jl

@@ -207,12 +207,15 @@ To obtain an axis-angle representation of any rotation, see [Conversion between
 end
 
 """
-    Body(; name, m = 1, r_cm, isroot = false, phi0 = zeros(3), phid0 = zeros(3), r_0=zeros(3), state_priority = 2, quat=false)
+    Body(; name, m = 1, r_cm, isroot = false, phi0 = zeros(3), phid0 = zeros(3), r_0 = zeros(3), state_priority = 2, quat = false, sparse_I = false)
 
 Representing a body with 3 translational and 3 rotational degrees-of-freedom.
 
 This component has a single frame, `frame_a`. To represent bodies with more than one frame, see [`BodyShape`](@ref), [`BodyCylinder`](@ref), [`BodyBox`](@ref).
 
+# Performance optimization
+- `sparse_I`: If `true`, the zero elements of the inerita matrix are considered "structurally zero", and this fact is used to optimize performance. When this option is enabled, the elements of the inertia matrix that were zero when the component was created cannot changed without reinstantiating the component. This performance optimization may be useful, e.g., when the inertia matrix is known to be diagonal.
+
 # Parameters
 - `m`: Mass
 - `r_cm`: Vector from `frame_a` to center of mass, resolved in `frame_a`
@@ -221,7 +224,6 @@ This component has a single frame, `frame_a`. To represent bodies with more than
 - `phi0`: Initial orientation, only applicable if `isroot = true` and `quat = false`
 - `phid0`: Initial angular velocity
 
-
 # Variables
 - `r_0`: Position vector from origin of world frame to origin of `frame_a`
 - `v_0`: Absolute velocity of `frame_a`, resolved in world frame (= D(r_0))
@@ -239,6 +241,7 @@ This component has a single frame, `frame_a`. To represent bodies with more than
               I_21 = 0,
               I_31 = 0,
               I_32 = 0,
+              sparse_I = false,
               isroot = false,
               state = false,
               sequence = [1,2,3],
@@ -295,6 +298,10 @@ This component has a single frame, `frame_a`. To represent bodies with more than
     @parameters render = render [description = "Render the component in animations"]
     # @parameters I[1:3, 1:3]=I [description="inertia tensor"]
 
+    if sparse_I
+        Isparsity = sparse(.!isequal.(0, [I_11 I_21 I_31; I_21 I_22 I_32; I_31 I_32 I_33]))
+    end
+
     @parameters I_11=I_11 [description = "Element (1,1) of inertia tensor"]
     @parameters I_22=I_22 [description = "Element (2,2) of inertia tensor"]
     @parameters I_33=I_33 [description = "Element (3,3) of inertia tensor"]
@@ -303,6 +310,9 @@ This component has a single frame, `frame_a`. To represent bodies with more than
     @parameters I_32=I_32 [description = "Element (3,2) of inertia tensor"]
 
     I = [I_11 I_21 I_31; I_21 I_22 I_32; I_31 I_32 I_33]
+    if sparse_I
+        I = I.*Isparsity
+    end
 
     r_0, v_0, a_0, g_0, w_a, z_a, r_cm = collect.((r_0, v_0, a_0, g_0, w_a, z_a, r_cm))
 
@@ -691,7 +701,7 @@ Rigid body with cylinder shape. The mass properties of the body (mass, center of
         frame_a = Frame()
         frame_b = Frame()
         translation = FixedTranslation(r = r)
-        body = Body(; m, r_cm, I_11 = I[1,1], I_22 = I[2,2], I_33 = I[3,3], I_21 = I[2,1], I_31 = I[3,1], I_32 = I[3,2], state, quat, isroot, sequence, neg_w)
+        body = Body(; m, r_cm, I_11 = I[1,1], I_22 = I[2,2], I_33 = I[3,3], I_21 = I[2,1], I_31 = I[3,1], I_32 = I[3,2], state, quat, isroot, sequence, neg_w, sparse_I=true)
     end
 
     @equations begin
@@ -821,7 +831,7 @@ Rigid body with box shape. The mass properties of the body (mass, center of mass
         frame_a = Frame()
         frame_b = Frame()
         translation = FixedTranslation(r = r)
-        body = Body(; m, r_cm, I_11 = I[1,1], I_22 = I[2,2], I_33 = I[3,3], I_21 = I[2,1], I_31 = I[3,1], I_32 = I[3,2], state, quat, isroot)
+        body = Body(; m, r_cm, I_11 = I[1,1], I_22 = I[2,2], I_33 = I[3,3], I_21 = I[2,1], I_31 = I[3,1], I_32 = I[3,2], state, quat, isroot, sparse_I = true)
     end
 
     @equations begin

src/robot/robot_components.jl

@@ -454,7 +454,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81)
         r4 = Revolute(n = [0, 1, 0], axisflange = true, isroot = false, radius=0.06, color=robot_orange)
         r5 = Revolute(n = [1, 0, 0], axisflange = true, isroot = false, radius=0.05, color=robot_orange)
         r6 = Revolute(n = [0, 1, 0], axisflange = true, isroot = false, radius=0.02, color=[0.5, 0.5, 0.5, 1])
-        b0 = BodyShape(r = [0, 0.351, 0],
+        b0 = BodyShape(sparse_I = true, r = [0, 0.351, 0],
                         shapefile = joinpath(path, "../../examples/resources/b0.stl"),
                        #    r_shape = [0, 0, 0],
                        #    lengthDirection = [1, 0, 0],
@@ -465,7 +465,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81)
                        radius = 0.3/2,
                        color = [0.5, 0.5, 0.5, 1],
                        m = 1)
-        b1 = BodyShape(r = [0, 0.324, 0.3],
+        b1 = BodyShape(sparse_I = true, r = [0, 0.324, 0.3],
                         shapefile = joinpath(path, "../../examples/resources/b1.stl"),
                        I_22 = 1.16,
                        #    lengthDirection = [1, 0, 0],
@@ -476,7 +476,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81)
                        radius = 0.2/2,
                        color = robot_orange,
                        m = 1)
-        b2 = BodyShape(r = [0, 0.65, 0],
+        b2 = BodyShape(sparse_I = true, r = [0, 0.65, 0],
                         shapefile = joinpath(path, "../../examples/resources/b2.stl"),
                        r_cm = [0.172, 0.205, 0],
                        m = 56.5,
@@ -492,7 +492,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81)
                        radius = 0.2/2,
                        color = robot_orange,
                        )
-        b3 = BodyShape(r = [0, 0.414, -0.155],
+        b3 = BodyShape(sparse_I = true, r = [0, 0.414, -0.155],
                         shapefile = joinpath(path, "../../examples/resources/b3.stl"),
                        r_cm = [0.064, -0.034, 0],
                        m = 26.4,
@@ -508,7 +508,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81)
                        radius = 0.15/2,
                        color = robot_orange,
                        )
-        b4 = BodyShape(r = [0, 0.186, 0],
+        b4 = BodyShape(sparse_I = true, r = [0, 0.186, 0],
                         shapefile = joinpath(path, "../../examples/resources/b4.stl"),
                        m = 28.7,
                        I_11 = 1.67,
@@ -522,7 +522,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81)
                        radius = 0.1/2,
                        color = robot_orange,
                        )
-        b5 = BodyShape(r = [0, 0.125, 0],
+        b5 = BodyShape(sparse_I = true, r = [0, 0.125, 0],
                         shapefile = joinpath(path, "../../examples/resources/b5.stl"),
                        m = 5.2,
                        I_11 = 1.25,
@@ -536,7 +536,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81)
                         radius = 0.1/2,
                         color = [0.5, 0.5, 0.5, 1],
                        )
-        b6 = BodyShape(r = [0, 0, 0],
+        b6 = BodyShape(sparse_I = true, r = [0, 0, 0],
                         shapefile = joinpath(path, "../../examples/resources/b6.stl"),
                        r_cm = [0.05, 0.05, 0.05],
                        m = 0.5,

src/wheels.jl

@@ -550,12 +550,14 @@ function RollingWheelSet(;
                     I_11 = I_long,
                     I_22 = I_long,
                     I_33 = I_axis,
+                    sparse_I = true,
                     render = false)
         body1 = Body(r_cm = [0, 0, 0],
                     m = m_wheel,
                     I_11 = I_long,
                     I_22 = I_long,
                     I_33 = I_axis,
+                    sparse_I = true,
                     render = false)
         axis1 = Rotational.Flange()
         axis2 = Rotational.Flange()