Fix center_of_mass allocations.

src/mechanism_algorithms.jl

@@ -35,11 +35,13 @@ function center_of_mass(state::MechanismState, itr)
     com = Point3D(frame, zero(SVector{3, T}))
     mass = zero(T)
     for body in itr
-        inertia = spatial_inertia(body)
-        if inertia.mass > 0
-            bodycom = transform_to_root(state, body, false) * center_of_mass(inertia)
-            com += inertia.mass * FreeVector3D(bodycom)
-            mass += inertia.mass
+        if body.inertia !== nothing
+            inertia = spatial_inertia(body)
+            if inertia.mass > 0
+                bodycom = transform_to_root(state, body, false) * center_of_mass(inertia)
+                com += inertia.mass * FreeVector3D(bodycom)
+                mass += inertia.mass
+            end
         end
     end
     com /= mass
@@ -51,7 +53,7 @@ $(SIGNATURES)
 
 Compute the center of mass of the whole `Mechanism` in the given state.
 """
-center_of_mass(state::MechanismState) = center_of_mass(state, non_root_bodies(state.mechanism))
+center_of_mass(state::MechanismState) = center_of_mass(state, bodies(state.mechanism))
 
 const geometric_jacobian_doc = """Compute a geometric Jacobian (also known as a
 basic, or spatial Jacobian) associated with a directed path in the `Mechanism`'s