diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 5a9db0232fe..f5989146335 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -926,6 +926,9 @@ func framesAlmostEqual(frame1, frame2 Frame, epsilon float64) (bool, error) { return false, nil } } + case *namedFrame: + f2 := frame2.(*namedFrame) + return framesAlmostEqual(f1.Frame, f2.Frame, epsilon) default: return false, fmt.Errorf("equality conditions not defined for %t", frame1) } diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index 6235dcc09b6..d3f9198fe20 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -40,8 +40,9 @@ type FrameSystemPart struct { type FrameSystem struct { name string world Frame // separate from the map of frames so it can be detached easily - frames map[string]Frame - parents map[string]string + frames map[string]Frame + parents map[string]string + // This excludes internal flattened frames. cachedBFSNames []string // flattenedModels maps component name → original SimpleModel for models that were @@ -57,6 +58,10 @@ type FrameSystem struct { // mimicFrames maps namespaced frame name → mimic info for flattened mimic joints. // composeTransforms uses this to derive inputs from the source frame. mimicFrames map[string]*mimicInfo + + // internalToComponent maps each flattened namespaced internal frame name + // (e.g., "arm1:joint1") to its owning component ("arm1"). + internalToComponent map[string]string } // mimicInfo describes a mimic joint relationship for a flattened frame. @@ -74,9 +79,10 @@ func NewEmptyFrameSystem(name string) *FrameSystem { world: worldFrame, frames: map[string]Frame{}, parents: map[string]string{}, - flattenedModels: map[string]*SimpleModel{}, - componentSchemas: map[string]*LinearInputsSchema{}, - mimicFrames: map[string]*mimicInfo{}, + flattenedModels: map[string]*SimpleModel{}, + componentSchemas: map[string]*LinearInputsSchema{}, + mimicFrames: map[string]*mimicInfo{}, + internalToComponent: map[string]string{}, } } @@ -154,17 +160,7 @@ func addPartToFS(fs *FrameSystem, part *FrameSystemPart) error { if err = fs.AddFrame(staticOffsetFrame, fs.Frame(part.FrameConfig.Parent())); err != nil { return err } - if err = fs.AddFrame(modelFrame, staticOffsetFrame); err != nil { - return err - } - - // Additionally flatten multi-DoF SimpleModels for intermediate frame parenting - if sm, ok := part.ModelFrame.(*SimpleModel); ok && len(sm.DoF()) > 0 { - if err = flattenModelIntoFS(fs, sm, part.FrameConfig.Name(), staticOffsetFrame); err != nil { - return err - } - } - return nil + return fs.AddFrame(modelFrame, staticOffsetFrame) } // addUnlinkedParts retries adding parts whose parents weren't available during the first @@ -216,7 +212,7 @@ func (sfs *FrameSystem) Parent(frame Frame) (Frame, error) { // If the raw parent is an internal flattened frame, return the component's // SimpleModel instead so callers see component-level names. - if componentName := sfs.componentForInternalFrame(parentName); componentName != "" { + if componentName := sfs.internalToComponent[parentName]; componentName != "" { if sm := sfs.Frame(componentName); sm != nil { return sm, nil } @@ -232,7 +228,10 @@ func (sfs *FrameSystem) frameExists(name string) bool { } // RemoveFrame will delete the given frame and all descendents from the frame system if it exists. +// When the frame is a flattened SimpleModel component, its namespaced internal +// frames and flattening metadata are also cleaned up. func (sfs *FrameSystem) RemoveFrame(frame Frame) { + sfs.cleanupFlattenedComponent(frame.Name()) sfs.removeFrameRecursive(frame) sfs.cachedBFSNames = bfsFrameNames(sfs) } @@ -249,6 +248,24 @@ func (sfs *FrameSystem) removeFrameRecursive(frame Frame) { } } +// cleanupFlattenedComponent removes the namespaced internal frames and +// flattening metadata for a component, if it is flattened. No-op if called on non-flattened component. +func (sfs *FrameSystem) cleanupFlattenedComponent(componentName string) { + model, ok := sfs.flattenedModels[componentName] + if !ok { + return + } + for _, internalName := range bfsFrameNames(model.internalFS) { + ns := componentName + ":" + internalName + delete(sfs.frames, ns) + delete(sfs.parents, ns) + delete(sfs.mimicFrames, ns) + delete(sfs.internalToComponent, ns) + } + delete(sfs.flattenedModels, componentName) + delete(sfs.componentSchemas, componentName) +} + // Frame returns the Frame which has the provided name. It returns nil if the frame is not found in the FraneSystem. func (sfs *FrameSystem) Frame(name string) Frame { if !sfs.frameExists(name) { @@ -281,7 +298,7 @@ func (sfs *FrameSystem) TracebackFrame(query Frame) ([]Frame, error) { // If the raw parent is an internal flattened frame, skip the entire internal // chain and jump to the component's SimpleModel. var nextParent Frame - if componentName := sfs.componentForInternalFrame(parentName); componentName != "" { + if componentName := sfs.internalToComponent[parentName]; componentName != "" { nextParent = sfs.Frame(componentName) } else { nextParent = sfs.Frame(parentName) @@ -294,48 +311,15 @@ func (sfs *FrameSystem) TracebackFrame(query Frame) ([]Frame, error) { return append([]Frame{query}, parents...), nil } -// componentForInternalFrame returns the component name that owns the given internal -// frame, or "" if the frame is not part of any flattened model. -func (sfs *FrameSystem) componentForInternalFrame(name string) string { - for componentName, schema := range sfs.componentSchemas { - for _, meta := range schema.metas { - if meta.frameName == name { - return componentName - } - } - } - return "" -} - -// FrameNames returns the list of frame names registered in the frame system. -// Internal frames from flattened models (namespaced with ":") are hidden from this list. -// Use cachedBFSNames directly for internal operations that need all frames. +// FrameNames returns the list of frame names registered in the frame system, +// in BFS order from world, excluding flattened-model internals. func (sfs *FrameSystem) FrameNames() []string { - if len(sfs.flattenedModels) == 0 { - return sfs.cachedBFSNames - } - internal := sfs.internalFrameNameSet() - result := make([]string, 0, len(sfs.cachedBFSNames)-len(internal)) - for _, name := range sfs.cachedBFSNames { - if !internal[name] { - result = append(result, name) - } - } - return result + return sfs.cachedBFSNames } -// internalFrameNameSet returns the set of namespaced frame names belonging to flattened models. -func (sfs *FrameSystem) internalFrameNameSet() map[string]bool { - result := make(map[string]bool) - for _, schema := range sfs.componentSchemas { - for _, name := range schema.FrameNamesInOrder() { - result[name] = true - } - } - return result -} - -// AddFrame sets an already defined Frame into the system. +// AddFrame sets an already defined Frame into the system. If the frame is (or +// wraps) a SimpleModel, its internal kinematic frames are flattened into the +// FS under the namespaced convention ":". func (sfs *FrameSystem) AddFrame(frame, parent Frame) error { // check to see if parent is in system if parent == nil { @@ -354,6 +338,12 @@ func (sfs *FrameSystem) AddFrame(frame, parent Frame) error { sfs.frames[frame.Name()] = frame sfs.parents[frame.Name()] = parent.Name() sfs.cachedBFSNames = bfsFrameNames(sfs) + + if sm := asFlattenableModel(frame); sm != nil { + if err := flattenModelIntoFS(sfs, sm, frame.Name(), parent); err != nil { + return err + } + } return nil } @@ -440,19 +430,15 @@ func (sfs *FrameSystem) MergeFrameSystem(systemToMerge *FrameSystem, attachTo Fr return NewFrameMissingError(attachTo.Name()) } - // make a map where the parent frame name is the key and the slice of children frames is the value - // Use cachedBFSNames to include internal flattened frames that are hidden from FrameNames(). + // Build a children map from the raw parents map (cachedBFSNames excludes + // internals; we want to traverse through them to reach external attachments + // parented to internal joints like "arm:joint3"). childrenMap := map[string][]Frame{} - for _, name := range systemToMerge.cachedBFSNames { + for name, rawParentName := range systemToMerge.parents { child := systemToMerge.Frame(name) if child == nil { continue } - // Use raw parents map to preserve internal frame structure (Parent() masks internal frames). - rawParentName, exists := systemToMerge.parents[name] - if !exists { - continue - } parent := systemToMerge.Frame(rawParentName) if parent == nil { continue @@ -460,37 +446,43 @@ func (sfs *FrameSystem) MergeFrameSystem(systemToMerge *FrameSystem, attachTo Fr childrenMap[parent.Name()] = append(childrenMap[parent.Name()], child) } - // add every frame from systemToMerge to the base frame system. + // Frames produced by flattening are (re-)created by AddFrame's auto-flatten + // when the component model is added, so we skip AddFrame'ing them here, + // but still traverse through them to reach external attachments below. + internalOfMerged := systemToMerge.internalToComponent + queue := []Frame{systemToMerge.World()} for len(queue) != 0 { parent := queue[0] queue = queue[1:] children := childrenMap[parent.Name()] + // Process non-internal children first so a component model is + // installed (and auto-flattened) before its sibling internal root is + // traversed. Otherwise external frames parented to "arm:jointN" would + // reach AddFrame before "arm:jointN" exists in the destination. + sort.SliceStable(children, func(i, j int) bool { + _, iInternal := internalOfMerged[children[i].Name()] + _, jInternal := internalOfMerged[children[j].Name()] + // non-internal (false) sorts before internal (true) + return !iInternal && jInternal + }) for _, c := range children { queue = append(queue, c) + if _, isInternal := internalOfMerged[c.Name()]; isInternal { + // Auto-flatten has already created (or will create) this frame + // when the component model is AddFrame'd. Don't double-add. + continue + } + destParent := parent if parent == systemToMerge.World() { - err := sfs.AddFrame(c, attachFrame) // attach c to the attachFrame - if err != nil { - return err - } - } else { - err := sfs.AddFrame(c, parent) - if err != nil { - return err - } + destParent = attachFrame + } + if err := sfs.AddFrame(c, destParent); err != nil { + return err } } } - // Copy flattening metadata from the merged system - for componentName, model := range systemToMerge.flattenedModels { - sfs.flattenedModels[componentName] = model - sfs.componentSchemas[componentName] = systemToMerge.componentSchemas[componentName] - } - for frameName, mi := range systemToMerge.mimicFrames { - sfs.mimicFrames[frameName] = mi - } - return nil } @@ -501,24 +493,16 @@ func (sfs *FrameSystem) MergeFrameSystem(systemToMerge *FrameSystem, attachTo Fr // the component's origin frame so that sibling internal frames (and anything parented to // them) are included in the subset. func (sfs *FrameSystem) FrameSystemSubset(newRoot Frame) (*FrameSystem, error) { - // If the root is a flattened SimpleModel, expand to the component's origin - // frame so that sibling internal frames and their descendants are included. - if _, isFlattenedComponent := sfs.flattenedModels[newRoot.Name()]; isFlattenedComponent { - originName := sfs.parents[newRoot.Name()] - if originFrame := sfs.Frame(originName); originFrame != nil { - newRoot = originFrame - } - } - newWorld := NewZeroStaticFrame(World) newFS := &FrameSystem{ name: newRoot.Name() + "_FS", world: newWorld, frames: map[string]Frame{}, parents: map[string]string{}, - flattenedModels: map[string]*SimpleModel{}, - componentSchemas: map[string]*LinearInputsSchema{}, - mimicFrames: map[string]*mimicInfo{}, + flattenedModels: map[string]*SimpleModel{}, + componentSchemas: map[string]*LinearInputsSchema{}, + mimicFrames: map[string]*mimicInfo{}, + internalToComponent: map[string]string{}, } rootFrame := sfs.Frame(newRoot.Name()) @@ -528,6 +512,31 @@ func (sfs *FrameSystem) FrameSystemSubset(newRoot Frame) (*FrameSystem, error) { newFS.frames[newRoot.Name()] = newRoot newFS.parents[newRoot.Name()] = newWorld.Name() + // If newRoot is a flattened SimpleModel component, its namespaced internal + // frames live as siblings (not descendants) under the component's real + // parent, so the traceParent walk below won't reach them. Pull them in + // directly, re-rooted at newWorld just like newRoot itself. + if model, isFlattenedComponent := sfs.flattenedModels[newRoot.Name()]; isFlattenedComponent { + for _, internalName := range bfsFrameNames(model.internalFS) { + namespaced := newRoot.Name() + ":" + internalName + internalFrame := sfs.Frame(namespaced) + if internalFrame == nil { + continue + } + rawParentName := sfs.parents[namespaced] + // The internal-root's raw parent is the component's attachTo in sfs; + // re-root it to newWorld in the subset. Other internals keep their + // raw parent, which is another namespaced internal of the same + // component (already being added in this loop). + destParentName := rawParentName + if _, parentIsInternal := sfs.internalToComponent[rawParentName]; !parentIsInternal { + destParentName = newWorld.Name() + } + newFS.frames[namespaced] = internalFrame + newFS.parents[namespaced] = destParentName + } + } + var traceParent func(Frame) bool traceParent = func(parent Frame) bool { // Determine to which frame system this frame and its parent should be added @@ -572,6 +581,11 @@ func (sfs *FrameSystem) FrameSystemSubset(newRoot Frame) (*FrameSystem, error) { newFS.mimicFrames[frameName] = mi } } + for internalName, componentName := range sfs.internalToComponent { + if newFS.frameExists(internalName) { + newFS.internalToComponent[internalName] = componentName + } + } newFS.cachedBFSNames = bfsFrameNames(newFS) return newFS, nil @@ -631,6 +645,10 @@ func (sfs *FrameSystem) ReplaceFrame(replacementFrame Frame) error { } replaceMeParent := sfs.Frame(rawParentName) + // If replaceMe is a flattened-model component, tear down its internals and + // metadata so the AddFrame below can auto-flatten the replacement cleanly. + sfs.cleanupFlattenedComponent(replaceMe.Name()) + // remove replaceMe from the frame system delete(sfs.frames, replaceMe.Name()) delete(sfs.parents, replaceMe.Name()) @@ -643,19 +661,9 @@ func (sfs *FrameSystem) ReplaceFrame(replacementFrame Frame) error { } } - // add replacementFrame to frame system with parent of replaceMe - if err := sfs.AddFrame(replacementFrame, replaceMeParent); err != nil { - return err - } - - // If replacing a flattened model's component, update the stored model - if sm, ok := replacementFrame.(*SimpleModel); ok { - if _, exists := sfs.flattenedModels[replacementFrame.Name()]; exists { - sfs.flattenedModels[replacementFrame.Name()] = sm - } - } - - return nil + // add replacementFrame to frame system with parent of replaceMe. AddFrame + // handles auto-flatten if replacementFrame is (or wraps) a SimpleModel. + return sfs.AddFrame(replacementFrame, replaceMeParent) } // Returns the relative pose between the parent and the destination frame. @@ -766,10 +774,7 @@ func (sfs *FrameSystem) MarshalJSON() ([]byte, error) { } // UnmarshalJSON parses a FrameSystem from JSON data. -// The flattened-model bookkeeping (flattenedModels, componentSchemas, -// mimicFrames) is not serialized. We regenerate it here via -// registerFlattenedModel, which is the same code path flattenModelIntoFS -// uses at construction. +// Frames are re-added in BFS order from World via AddFrame. func (sfs *FrameSystem) UnmarshalJSON(data []byte) error { type serializableFrameSystem struct { Name string `json:"name"` @@ -796,55 +801,71 @@ func (sfs *FrameSystem) UnmarshalJSON(data []byte) error { frameMap[name] = frame } - sfs.frames = frameMap - sfs.parents = serFS.Parents - sfs.world = worldFrame - sfs.name = serFS.Name - sfs.cachedBFSNames = bfsFrameNames(sfs) - sfs.flattenedModels = map[string]*SimpleModel{} - sfs.componentSchemas = map[string]*LinearInputsSchema{} - sfs.mimicFrames = map[string]*mimicInfo{} - for name, frame := range sfs.frames { - sm := asFlattenableModel(frame) - if sm == nil { - continue + // Internals will be re-created by AddFrame's auto-flatten when their + // component model is added, so they're skipped during the walk below. + internals := map[string]bool{} + for componentName, frame := range frameMap { + if model := asFlattenableModel(frame); model != nil { + for _, internalName := range bfsFrameNames(model.internalFS) { + internals[componentName+":"+internalName] = true + } } - // Only regenerate flattened internals if the model was flattened into the outer FS at - // construction time, i.e. its internal frames are present under the - // namespaced names. A SimpleModel added directly via AddFrame has no - // such internals and must not be registered. - if wasFlattened(sfs, name, sm) { - sfs.registerFlattenedModel(name, sm) + } + + childrenOf := map[string][]string{} + for name, parent := range serFS.Parents { + if _, ok := frameMap[name]; !ok { + return fmt.Errorf("cannot deserialize frame system: parents map references unknown frame %q", name) } + childrenOf[parent] = append(childrenOf[parent], name) + } + for k := range childrenOf { + sort.Strings(childrenOf[k]) } - return nil -} -// wasFlattened reports whether the model at componentName was flattened into -// the outer FS (its bfs-enumerated internal frames exist under the -// componentName:internalName convention). -func wasFlattened(sfs *FrameSystem, componentName string, model *SimpleModel) bool { - internalNames := bfsFrameNames(model.internalFS) - if len(internalNames) == 0 { - return false + *sfs = *NewEmptyFrameSystem(serFS.Name) + sfs.world = worldFrame + + visited := make(map[string]bool, len(frameMap)) + queue := []string{World} + for len(queue) > 0 { + parent := queue[0] + queue = queue[1:] + for _, name := range childrenOf[parent] { + queue = append(queue, name) + visited[name] = true + if internals[name] { + continue + } + parentFrame := sfs.Frame(parent) + if parentFrame == nil { + return NewFrameMissingError(parent) + } + if err := sfs.AddFrame(frameMap[name], parentFrame); err != nil { + return err + } + } } - for _, internalName := range internalNames { - if _, ok := sfs.frames[componentName+":"+internalName]; !ok { - return false + + if len(visited) != len(frameMap) { + var orphans []string + for name := range frameMap { + if !visited[name] { + orphans = append(orphans, name) + } } + sort.Strings(orphans) + return fmt.Errorf("cannot deserialize frame system: frames not reachable from world: %v", orphans) } - return true + return nil } -// asFlattenableModel returns the underlying *SimpleModel if frame is (or wraps) -// a SimpleModel with at least one DoF, else nil. Zero-DoF models are not -// flattened (see addPartToFS). +// asFlattenableModel returns the underlying *SimpleModel if frame is (or +// wraps) a SimpleModel, else nil. func asFlattenableModel(frame Frame) *SimpleModel { switch f := frame.(type) { case *SimpleModel: - if len(f.DoF()) > 0 { - return f - } + return f case *namedFrame: return asFlattenableModel(f.Frame) } @@ -1193,21 +1214,20 @@ func TopologicallySortParts(parts []*FrameSystemPart) ([]*FrameSystemPart, []*Fr return topoSortedParts, unlinkedParts } -// flattenModelIntoFS unpacks a SimpleModel's internal frames into the outer FrameSystem. -// Each internal frame is renamed with a namespace prefix (componentName:internalName). -// Mimic joint info is stored in the FS's mimicFrames map for use by composeTransforms. +// flattenModelIntoFS unpacks a SimpleModel's internal frames into the outer +// FrameSystem under namespaced names (componentName:internalName) and +// populates the FS's flattenedModels, componentSchemas, and mimicFrames +// bookkeeping. Called from AddFrame when the added frame is (or wraps) a +// multi-DoF SimpleModel. func flattenModelIntoFS(outerFS *FrameSystem, model *SimpleModel, componentName string, attachTo Frame) error { internalFS := model.internalFS - internalNames := bfsFrameNames(internalFS) - - for _, internalName := range internalNames { + for _, internalName := range bfsFrameNames(internalFS) { namespacedName := componentName + ":" + internalName innerFrame := internalFS.Frame(internalName) if innerFrame == nil { return NewFrameMissingError(internalName) } - // Determine the parent in the outer FS internalParentName := internalFS.parents[internalName] var parentFrame Frame if internalParentName == World { @@ -1220,28 +1240,21 @@ func flattenModelIntoFS(outerFS *FrameSystem, model *SimpleModel, componentName } } - wrappedFrame := NewNamedFrame(innerFrame, namespacedName) - if err := outerFS.AddFrame(wrappedFrame, parentFrame); err != nil { + // Tag as an internal BEFORE AddFrame so the bfsFrameNames cache + // recomputed inside AddFrame already knows to filter it out. + outerFS.internalToComponent[namespacedName] = componentName + if err := outerFS.AddFrame(NewNamedFrame(innerFrame, namespacedName), parentFrame); err != nil { return err } } - outerFS.registerFlattenedModel(componentName, model) - return nil -} - -// registerFlattenedModel populates the three derived maps (flattenedModels, -// componentSchemas, mimicFrames) for a flattened model. The namespaced -// componentName:* frames must already exist in sfs.frames so the schema metas -// can be bound to them. Called from both flattenModelIntoFS and UnmarshalJSON -func (sfs *FrameSystem) registerFlattenedModel(componentName string, model *SimpleModel) { - sfs.flattenedModels[componentName] = model + outerFS.flattenedModels[componentName] = model for internalName, mm := range model.mimicMappings { if mm == nil { continue } - sfs.mimicFrames[componentName+":"+internalName] = &mimicInfo{ + outerFS.mimicFrames[componentName+":"+internalName] = &mimicInfo{ sourceFrameName: componentName + ":" + mm.sourceFrameName, multiplier: mm.valueMultiplier, offset: mm.valueOffset, @@ -1254,14 +1267,18 @@ func (sfs *FrameSystem) registerFlattenedModel(componentName string, model *Simp frameName: componentName + ":" + meta.frameName, offset: meta.offset, dof: meta.dof, - frame: sfs.Frame(componentName + ":" + meta.frameName), + frame: outerFS.Frame(componentName + ":" + meta.frameName), }) } - sfs.componentSchemas[componentName] = &LinearInputsSchema{metas: namespacedMetas} + outerFS.componentSchemas[componentName] = &LinearInputsSchema{metas: namespacedMetas} + return nil } -// bfsFrameNames returns frame names in BFS order from world. Children at each level are -// sorted alphabetically for determinism. +// bfsFrameNames returns frame names in BFS order from world, excluding +// flattened-model internals. Internals are traversed (so external frames +// parented to internal joints are still discovered) but are not emitted in +// the output — they're an implementation detail of the component model. +// Children at each level are sorted alphabetically for determinism. func bfsFrameNames(fs *FrameSystem) []string { childrenOf := map[string][]string{} for name := range fs.frames { @@ -1282,7 +1299,9 @@ func bfsFrameNames(fs *FrameSystem) []string { cur := queue[0] queue = queue[1:] if cur != World { - result = append(result, cur) + if _, internal := fs.internalToComponent[cur]; !internal { + result = append(result, cur) + } } queue = append(queue, childrenOf[cur]...) } @@ -1290,7 +1309,9 @@ func bfsFrameNames(fs *FrameSystem) []string { } // cloneFrameSystem creates a deep copy of a FrameSystem by cloning each frame individually -// and rebuilding the parent-child relationships. +// and rebuilding the parent-child relationships. Flattened-model internals are +// re-created by AddFrame's auto-flatten when their component model is cloned, +// so we skip them here. func cloneFrameSystem(fs *FrameSystem) (*FrameSystem, error) { newFS := NewEmptyFrameSystem(fs.name) for _, name := range fs.FrameNames() { @@ -1299,11 +1320,18 @@ func cloneFrameSystem(fs *FrameSystem) (*FrameSystem, error) { if err != nil { return nil, fmt.Errorf("cloning frame %q: %w", name, err) } - parent, err := fs.Parent(frame) - if err != nil { - return nil, err + // Use the raw parent (not fs.Parent, which masks internal frames to + // the component model) so external frames parented to internal joints + // preserve that attachment. + rawParentName, ok := fs.parents[name] + if !ok { + return nil, NewParentFrameNilError(name) + } + parentFrame := newFS.Frame(rawParentName) + if parentFrame == nil { + return nil, NewFrameMissingError(rawParentName) } - if err := newFS.AddFrame(clonedFrame, newFS.Frame(parent.Name())); err != nil { + if err := newFS.AddFrame(clonedFrame, parentFrame); err != nil { return nil, err } }