RSDK-14012: Have FrameSystems walk frames/geometries to reset mesh caches.

motionplan/armplanning/api.go

@@ -284,29 +284,9 @@ func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanR
 // fine — failures here are logged and swallowed rather than surfaced as a
 // planning error.
 func resetMeshCaches(ctx context.Context, logger logging.Logger, request *PlanRequest) {
-	visit := func(geometry spatialmath.Geometry) {
-		if mesh, ok := geometry.(*spatialmath.Mesh); ok {
-			mesh.ResetCache()
-		}
-	}
-
-	if request.StartState != nil && request.StartState.structuredConfiguration != nil {
-		frameSystemGeometries, err := referenceframe.FrameSystemGeometries(
-			request.FrameSystem,
-			request.StartState.structuredConfiguration,
-		)
-		if err != nil {
-			logger.CDebugf(ctx, "resetMeshCaches: skipping robot geometries: %v", err)
-			// Not expected to happen, but we still want to try walking the world state. Rely on
-			// ranging over a nil slice to be a no-op.
-		}
-
-		for _, geometriesInFrame := range frameSystemGeometries {
-			for _, geometry := range geometriesInFrame.Geometries() {
-				visit(geometry)
-			}
-		}
-	}
+	// The request frame system holds mesh geometries that are shared are expected to persist across an entire
+	// process lifetime.
+	request.FrameSystem.ResetCaches()
 
 	if request.WorldState != nil {
 		// Right now, the world state is getting entirely thrown away anyways. It's always a part of
@@ -322,7 +302,9 @@ func resetMeshCaches(ctx context.Context, logger logging.Logger, request *PlanRe
 		}
 
 		for _, geometry := range obstacles.Geometries() {
-			visit(geometry)
+			if mesh, ok := geometry.(*spatialmath.Mesh); ok {
+				mesh.ResetCache()
+			}
 		}
 	}
 }

referenceframe/frame_system.go

@@ -87,6 +87,44 @@ func NewEmptyFrameSystem(name string) *FrameSystem {
 	}
 }
 
+// ResetCaches walks the frame system for all mesh geometries and invokes their `ResetCache`
+// method. This is only required to avoid piling up too much memory usage with cache values are low
+// value. Cached values are important in the context of a single motion plan request, but become
+// increasingly irrelevant when degrees of freedom change.
+func (sfs *FrameSystem) ResetCaches() {
+	resetMesh := func(geom spatial.Geometry) {
+		if mesh, isMesh := geom.(*spatial.Mesh); isMesh {
+			mesh.ResetCache()
+		}
+	}
+
+	var resetFrame func(Frame)
+	resetFrame = func(frameI Frame) {
+		switch frame := frameI.(type) {
+		case *staticFrame:
+			resetMesh(frame.geometry)
+		case *tailGeometryStaticFrame:
+			resetMesh(frame.geometry)
+		case *translationalFrame:
+			resetMesh(frame.geometry)
+		case *rotationalFrame:
+			// no geometries
+		case *namedFrame:
+			resetFrame(frameI)
+		case *poseFrame:
+			for _, geom := range frame.geometries {
+				resetMesh(geom)
+			}
+		case *SimpleModel:
+			frame.internalFS.ResetCaches()
+		}
+	}
+
+	for _, frameI := range sfs.frames {
+		resetFrame(frameI)
+	}
+}
+
 // resolveFrameInputs is a fallback for when linearInputs.Get(frameName) returns nil.
 // It checks if frameName belongs to a flattened model and, if so, extracts the right
 // slice from a component-name-keyed entry in the LinearInputs.