diff --git a/motionplan/armplanning/data/plan_request_sample.json b/motionplan/armplanning/data/plan_request_sample.json
index 42f1fc71e96..2b79f581734 100644
--- a/motionplan/armplanning/data/plan_request_sample.json
+++ b/motionplan/armplanning/data/plan_request_sample.json
@@ -26,260 +26,263 @@
         "frame_type": "model",
         "frame": {
           "name": "xArm6",
-          "links": [
-            {
-              "id": "base",
-              "translation": {
-                "X": 0,
-                "Y": 0,
-                "Z": 0
-              },
-              "orientation": null,
-              "parent": "world"
-            },
-            {
-              "id": "base_top",
-              "translation": {
-                "X": 0,
-                "Y": 0,
-                "Z": 267
+          "model": {
+            "name": "xArm6",
+            "links": [
+              {
+                "id": "base",
+                "translation": {
+                  "X": 0,
+                  "Y": 0,
+                  "Z": 0
+                },
+                "orientation": null,
+                "parent": "world"
               },
-              "orientation": null,
-              "geometry": {
-                "type": "",
-                "x": 0,
-                "y": 0,
-                "z": 0,
-                "r": 50,
-                "l": 320,
+              {
+                "id": "base_top",
                 "translation": {
                   "X": 0,
                   "Y": 0,
-                  "Z": 160
+                  "Z": 267
                 },
-                "orientation": {
-                  "type": ""
+                "orientation": null,
+                "geometry": {
+                  "type": "",
+                  "x": 0,
+                  "y": 0,
+                  "z": 0,
+                  "r": 50,
+                  "l": 320,
+                  "translation": {
+                    "X": 0,
+                    "Y": 0,
+                    "Z": 160
+                  },
+                  "orientation": {
+                    "type": ""
+                  },
+                  "Label": ""
                 },
-                "Label": ""
-              },
-              "parent": "waist"
-            },
-            {
-              "id": "upper_arm",
-              "translation": {
-                "X": 53.5,
-                "Y": 0,
-                "Z": 284.5
+                "parent": "waist"
               },
-              "orientation": null,
-              "geometry": {
-                "type": "",
-                "x": 110,
-                "y": 190,
-                "z": 370,
-                "r": 0,
-                "l": 0,
+              {
+                "id": "upper_arm",
                 "translation": {
-                  "X": 0,
+                  "X": 53.5,
                   "Y": 0,
-                  "Z": 135
+                  "Z": 284.5
                 },
-                "orientation": {
-                  "type": ""
+                "orientation": null,
+                "geometry": {
+                  "type": "",
+                  "x": 110,
+                  "y": 190,
+                  "z": 370,
+                  "r": 0,
+                  "l": 0,
+                  "translation": {
+                    "X": 0,
+                    "Y": 0,
+                    "Z": 135
+                  },
+                  "orientation": {
+                    "type": ""
+                  },
+                  "Label": ""
                 },
-                "Label": ""
+                "parent": "shoulder"
               },
-              "parent": "shoulder"
-            },
-            {
-              "id": "upper_forearm",
-              "translation": {
-                "X": 77.5,
-                "Y": 0,
-                "Z": -172.5
+              {
+                "id": "upper_forearm",
+                "translation": {
+                  "X": 77.5,
+                  "Y": 0,
+                  "Z": -172.5
+                },
+                "orientation": null,
+                "geometry": {
+                  "type": "",
+                  "x": 100,
+                  "y": 190,
+                  "z": 250,
+                  "r": 0,
+                  "l": 0,
+                  "translation": {
+                    "X": 49.49,
+                    "Y": 0,
+                    "Z": -49.49
+                  },
+                  "orientation": {
+                    "type": "ov_degrees",
+                    "value": {
+                      "th": 0,
+                      "x": 0.707106,
+                      "y": 0,
+                      "z": -0.707106
+                    }
+                  },
+                  "Label": ""
+                },
+                "parent": "elbow"
               },
-              "orientation": null,
-              "geometry": {
-                "type": "",
-                "x": 100,
-                "y": 190,
-                "z": 250,
-                "r": 0,
-                "l": 0,
+              {
+                "id": "lower_forearm",
                 "translation": {
-                  "X": 49.49,
+                  "X": 0,
                   "Y": 0,
-                  "Z": -49.49
+                  "Z": -170
                 },
-                "orientation": {
-                  "type": "ov_degrees",
-                  "value": {
-                    "th": 0,
-                    "x": 0.707106,
-                    "y": 0,
-                    "z": -0.707106
-                  }
+                "orientation": null,
+                "geometry": {
+                  "type": "",
+                  "x": 0,
+                  "y": 0,
+                  "z": 0,
+                  "r": 45,
+                  "l": 285,
+                  "translation": {
+                    "X": 0,
+                    "Y": -27.5,
+                    "Z": -104.8
+                  },
+                  "orientation": {
+                    "type": "ov_degrees",
+                    "value": {
+                      "th": -90,
+                      "x": 0,
+                      "y": 0.2537568,
+                      "z": 0.9672615
+                    }
+                  },
+                  "Label": ""
                 },
-                "Label": ""
+                "parent": "forearm_rot"
               },
-              "parent": "elbow"
-            },
-            {
-              "id": "lower_forearm",
-              "translation": {
-                "X": 0,
-                "Y": 0,
-                "Z": -170
+              {
+                "id": "wrist_link",
+                "translation": {
+                  "X": 76,
+                  "Y": 0,
+                  "Z": -97
+                },
+                "orientation": null,
+                "geometry": {
+                  "type": "",
+                  "x": 150,
+                  "y": 100,
+                  "z": 135,
+                  "r": 0,
+                  "l": 0,
+                  "translation": {
+                    "X": 75,
+                    "Y": 10,
+                    "Z": -67.5
+                  },
+                  "orientation": {
+                    "type": ""
+                  },
+                  "Label": ""
+                },
+                "parent": "wrist"
               },
-              "orientation": null,
-              "geometry": {
-                "type": "",
-                "x": 0,
-                "y": 0,
-                "z": 0,
-                "r": 45,
-                "l": 285,
+              {
+                "id": "gripper_mount",
                 "translation": {
                   "X": 0,
-                  "Y": -27.5,
-                  "Z": -104.8
+                  "Y": 0,
+                  "Z": 0
                 },
                 "orientation": {
                   "type": "ov_degrees",
                   "value": {
-                    "th": -90,
+                    "th": 0,
                     "x": 0,
-                    "y": 0.2537568,
-                    "z": 0.9672615
+                    "y": 0,
+                    "z": -1
                   }
                 },
-                "Label": ""
-              },
-              "parent": "forearm_rot"
-            },
-            {
-              "id": "wrist_link",
-              "translation": {
-                "X": 76,
-                "Y": 0,
-                "Z": -97
-              },
-              "orientation": null,
-              "geometry": {
-                "type": "",
-                "x": 150,
-                "y": 100,
-                "z": 135,
-                "r": 0,
-                "l": 0,
-                "translation": {
-                  "X": 75,
-                  "Y": 10,
-                  "Z": -67.5
-                },
-                "orientation": {
-                  "type": ""
+                "parent": "gripper_rot"
+              }
+            ],
+            "joints": [
+              {
+                "id": "waist",
+                "type": "revolute",
+                "parent": "base",
+                "axis": {
+                  "X": 0,
+                  "Y": 0,
+                  "Z": 1
                 },
-                "Label": ""
-              },
-              "parent": "wrist"
-            },
-            {
-              "id": "gripper_mount",
-              "translation": {
-                "X": 0,
-                "Y": 0,
-                "Z": 0
-              },
-              "orientation": {
-                "type": "ov_degrees",
-                "value": {
-                  "th": 0,
-                  "x": 0,
-                  "y": 0,
-                  "z": -1
-                }
-              },
-              "parent": "gripper_rot"
-            }
-          ],
-          "joints": [
-            {
-              "id": "waist",
-              "type": "revolute",
-              "parent": "base",
-              "axis": {
-                "X": 0,
-                "Y": 0,
-                "Z": 1
+                "max": 359,
+                "min": -359
               },
-              "max": 359,
-              "min": -359
-            },
-            {
-              "id": "shoulder",
-              "type": "revolute",
-              "parent": "base_top",
-              "axis": {
-                "X": 0,
-                "Y": 1,
-                "Z": 0
-              },
-              "max": 120,
-              "min": -118
-            },
-            {
-              "id": "elbow",
-              "type": "revolute",
-              "parent": "upper_arm",
-              "axis": {
-                "X": 0,
-                "Y": 1,
-                "Z": 0
+              {
+                "id": "shoulder",
+                "type": "revolute",
+                "parent": "base_top",
+                "axis": {
+                  "X": 0,
+                  "Y": 1,
+                  "Z": 0
+                },
+                "max": 120,
+                "min": -118
               },
-              "max": 10,
-              "min": -225
-            },
-            {
-              "id": "forearm_rot",
-              "type": "revolute",
-              "parent": "upper_forearm",
-              "axis": {
-                "X": 0,
-                "Y": 0,
-                "Z": -1
+              {
+                "id": "elbow",
+                "type": "revolute",
+                "parent": "upper_arm",
+                "axis": {
+                  "X": 0,
+                  "Y": 1,
+                  "Z": 0
+                },
+                "max": 10,
+                "min": -225
               },
-              "max": 359,
-              "min": -359
-            },
-            {
-              "id": "wrist",
-              "type": "revolute",
-              "parent": "lower_forearm",
-              "axis": {
-                "X": 0,
-                "Y": 1,
-                "Z": 0
+              {
+                "id": "forearm_rot",
+                "type": "revolute",
+                "parent": "upper_forearm",
+                "axis": {
+                  "X": 0,
+                  "Y": 0,
+                  "Z": -1
+                },
+                "max": 359,
+                "min": -359
               },
-              "max": 179,
-              "min": -97
-            },
-            {
-              "id": "gripper_rot",
-              "type": "revolute",
-              "parent": "wrist_link",
-              "axis": {
-                "X": 0,
-                "Y": 0,
-                "Z": -1
+              {
+                "id": "wrist",
+                "type": "revolute",
+                "parent": "lower_forearm",
+                "axis": {
+                  "X": 0,
+                  "Y": 1,
+                  "Z": 0
+                },
+                "max": 179,
+                "min": -97
               },
-              "max": 359,
-              "min": -359
+              {
+                "id": "gripper_rot",
+                "type": "revolute",
+                "parent": "wrist_link",
+                "axis": {
+                  "X": 0,
+                  "Y": 0,
+                  "Z": -1
+                },
+                "max": 359,
+                "min": -359
+              }
+            ],
+            "OriginalFile": {
+              "Bytes": "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",
+              "Extension": "json"
             }
-          ],
-          "OriginalFile": {
-            "Bytes": "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",
-            "Extension": "json"
           }
         }
       },
@@ -327,7 +330,10 @@
         }
       }
     },
-    "parents": null
+    "parents": {
+      "xArm6": "world",
+      "xArmVgripper": "xArm6"
+    }
   },
   "goals": [
     {
@@ -428,7 +434,7 @@
     "logging_interval": 0,
     "timeout": 300,
     "smooth_iter": 100,
-    "num_threads": 8,
+    "num_threads": 6,
     "goal_threshold": 0.1,
     "plan_iter": 1500,
     "frame_step": 0.01,
diff --git a/referenceframe/frame.go b/referenceframe/frame.go
index 995a1a1f28f..431291f4b9c 100644
--- a/referenceframe/frame.go
+++ b/referenceframe/frame.go
@@ -9,6 +9,7 @@ import (
 	"fmt"
 	"math"
 	"math/rand"
+	"reflect"
 	"strings"
 
 	"github.com/golang/geo/r3"
@@ -29,6 +30,15 @@ type Limit struct {
 	Max float64
 }
 
+func limitAlmostEqual(limit1, limit2 Limit, epsilon float64) bool {
+	if math.Abs(limit1.Max-limit2.Max) > epsilon {
+		return false
+	} else if math.Abs(limit1.Min-limit2.Min) > epsilon {
+		return false
+	}
+	return true
+}
+
 // RestrictedRandomFrameInputs will produce a list of valid, in-bounds inputs for the frame.
 // The range of selection is restricted to `restrictionPercent` percent of the limits, and the
 // selection frame is centered at reference.
@@ -93,6 +103,18 @@ type Limited interface {
 	DoF() []Limit
 }
 
+func limitsAlmostEqual(limits1, limits2 []Limit, epsilon float64) bool {
+	if len(limits1) != len(limits2) {
+		return false
+	}
+	for i, limit := range limits1 {
+		if !limitAlmostEqual(limit, limits2[i], epsilon) {
+			return false
+		}
+	}
+	return true
+}
+
 // Frame represents a reference frame, e.g. an arm, a joint, a gripper, a board, etc.
 type Frame interface {
 	Limited
@@ -193,6 +215,17 @@ func (sf *tailGeometryStaticFrame) Geometries(input []Input) (*GeometriesInFrame
 	return NewGeometriesInFrame(sf.name, []spatial.Geometry{newGeom}), nil
 }
 
+func (sf *tailGeometryStaticFrame) UnmarshalJSON(data []byte) error {
+	var inner staticFrame
+
+	err := json.Unmarshal(data, &inner)
+	if err != nil {
+		return err
+	}
+	sf.staticFrame = &inner
+	return nil
+}
+
 // namedFrame is used to change the name of a frame.
 type namedFrame struct {
 	Frame
@@ -501,7 +534,9 @@ func (rf *rotationalFrame) UnmarshalJSON(data []byte) error {
 		return err
 	}
 
-	rf.baseFrame = &baseFrame{name: rf.Name(), limits: []Limit{{Min: cfg.Min, Max: cfg.Max}}}
+	rf.baseFrame = &baseFrame{name: cfg.ID, limits: []Limit{
+		{Min: utils.DegToRad(cfg.Min), Max: utils.DegToRad(cfg.Max)},
+	}}
 	rotAxis := cfg.Axis.ParseConfig()
 	rf.rotAxis = r3.Vector{X: rotAxis.RX, Y: rotAxis.RY, Z: rotAxis.RZ}
 	return nil
@@ -630,3 +665,85 @@ func PoseToInputs(p spatial.Pose) []Input {
 		p.Orientation().OrientationVectorRadians().Theta,
 	})
 }
+
+// Determine whether two frames are (nearly) identical. For now, we only support implementers of
+// the frame interface that are registered (see register.go). However, this is not automatic, so
+// if a new implementer is registered manually in register.go, its case should be added here.
+//
+// NOTE: for ease, this function only takes one epsilon parameter because we have yet
+// to see a case of quantity where we want accept different levels of floating point error.
+// If the time comes where we want a different allowance for limits, vectors, and geometries,
+// this function should be changed accordingly.
+func framesAlmostEqual(frame1, frame2 Frame, epsilon float64) (bool, error) {
+	switch {
+	case reflect.TypeOf(frame1) != reflect.TypeOf(frame2):
+		return false, nil
+	case frame1.Name() != frame2.Name():
+		return false, nil
+	case !limitsAlmostEqual(frame1.DoF(), frame2.DoF(), epsilon):
+		return false, nil
+	default:
+	}
+
+	if frame1 == nil {
+		return frame2 == nil, nil
+	} else if frame2 == nil {
+		return false, nil
+	}
+
+	switch f1 := frame1.(type) {
+	case *staticFrame:
+		f2 := frame2.(*staticFrame)
+		switch {
+		case !spatial.PoseAlmostEqual(f1.transform, f2.transform):
+			return false, nil
+		case !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry):
+			return false, nil
+		default:
+		}
+	case *rotationalFrame:
+		f2 := frame2.(*rotationalFrame)
+		if !spatial.R3VectorAlmostEqual(f1.rotAxis, f2.rotAxis, epsilon) {
+			return false, nil
+		}
+	case *translationalFrame:
+		f2 := frame2.(*translationalFrame)
+		switch {
+		case !spatial.R3VectorAlmostEqual(f1.transAxis, f2.transAxis, epsilon):
+			return false, nil
+		case !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry):
+			return false, nil
+		default:
+		}
+	case *tailGeometryStaticFrame:
+		f2 := frame2.(*tailGeometryStaticFrame)
+		switch {
+		case f1.staticFrame == nil:
+			return f2.staticFrame == nil, nil
+		case f2.staticFrame == nil:
+			return f1.staticFrame == nil, nil
+		default:
+			return framesAlmostEqual(f1.staticFrame, f2.staticFrame, epsilon)
+		}
+	case *SimpleModel:
+		f2 := frame2.(*SimpleModel)
+		ordTransforms1 := f1.OrdTransforms
+		ordTransforms2 := f2.OrdTransforms
+		if len(ordTransforms1) != len(ordTransforms2) {
+			return false, nil
+		} else {
+			for i, f := range ordTransforms1 {
+				frameEquality, err := framesAlmostEqual(f, ordTransforms2[i], epsilon)
+				if err != nil {
+					return false, err
+				}
+				if !frameEquality {
+					return false, nil
+				}
+			}
+		}
+	default:
+		return false, fmt.Errorf("equality conditions not defined for %t", frame1)
+	}
+	return true, nil
+}
diff --git a/referenceframe/frame_json.go b/referenceframe/frame_json.go
index 067e78b3c0a..5789858001f 100644
--- a/referenceframe/frame_json.go
+++ b/referenceframe/frame_json.go
@@ -199,16 +199,23 @@ func jsonToFrame(data json.RawMessage) (Frame, error) {
 	if err := json.Unmarshal(sF["frame_type"], &frameType); err != nil {
 		return nil, err
 	}
+	if _, ok := sF["frame"]; !ok {
+		return nil, fmt.Errorf("no frame data found for frame, type was %s", frameType)
+	}
 
 	implementer, ok := registeredFrameImplementers[frameType]
 	if !ok {
 		return nil, fmt.Errorf("%s is not a registered Frame implementation", frameType)
 	}
 	frameZeroStruct := reflect.New(implementer).Elem()
-	if err := json.Unmarshal(sF["frame"], frameZeroStruct.Addr().Interface()); err != nil {
+	frame := frameZeroStruct.Addr().Interface()
+	frameI, err := utils.AssertType[Frame](frame)
+	if err != nil {
+		return nil, err
+	}
+	if err := json.Unmarshal(sF["frame"], frame); err != nil {
 		return nil, err
 	}
-	frame := frameZeroStruct.Addr().Interface()
 
-	return utils.AssertType[Frame](frame)
+	return frameI, nil
 }
diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go
index 009bf2fc224..c7afb890cb7 100644
--- a/referenceframe/frame_system.go
+++ b/referenceframe/frame_system.go
@@ -3,6 +3,7 @@ package referenceframe
 import (
 	"encoding/json"
 	"fmt"
+	"reflect"
 	"sort"
 
 	"github.com/golang/geo/r3"
@@ -458,9 +459,10 @@ func (sfs *FrameSystem) MarshalJSON() ([]byte, error) {
 		typedFrames[name] = frameJSON
 	}
 	serializedFS := serializableFrameSystem{
-		Name:   sfs.name,
-		World:  worldFrameJSON,
-		Frames: typedFrames,
+		Name:    sfs.name,
+		World:   worldFrameJSON,
+		Frames:  typedFrames,
+		Parents: sfs.parents,
 	}
 	return json.Marshal(serializedFS)
 }
@@ -588,7 +590,7 @@ func (part *FrameSystemPart) ToProtobuf() (*pb.FrameSystemConfig, error) {
 	if err != nil {
 		return nil, err
 	}
-	var modelJSON map[string]interface{}
+	var modelJSON SimpleModel
 	if part.ModelFrame != nil {
 		bytes, err := part.ModelFrame.MarshalJSON()
 		if err != nil {
@@ -599,7 +601,7 @@ func (part *FrameSystemPart) ToProtobuf() (*pb.FrameSystemConfig, error) {
 			return nil, err
 		}
 	}
-	kinematics, err := protoutils.StructToStructPb(modelJSON)
+	kinematics, err := protoutils.StructToStructPb(modelJSON.modelConfig)
 	if err != nil {
 		return nil, err
 	}
@@ -753,3 +755,40 @@ func TopologicallySortParts(parts []*FrameSystemPart) ([]*FrameSystemPart, error
 	}
 	return topoSortedParts, nil
 }
+
+func frameSystemsAlmostEqual(fs1, fs2 *FrameSystem, epsilon float64) (bool, error) {
+	if fs1.Name() != fs2.Name() {
+		return false, nil
+	}
+
+	worldFrameEquality, err := framesAlmostEqual(fs1.World(), fs2.World(), epsilon)
+	if err != nil {
+		return false, err
+	}
+	if !worldFrameEquality {
+		return false, nil
+	}
+
+	if !reflect.DeepEqual(fs1.parents, fs2.parents) {
+		return false, nil
+	}
+
+	if len(fs1.FrameNames()) != len(fs2.FrameNames()) {
+		return false, nil
+	}
+
+	for frameName, frame := range fs1.frames {
+		frame2, ok := fs2.frames[frameName]
+		if !ok {
+			return false, nil
+		}
+		frameEquality, err := framesAlmostEqual(frame, frame2, epsilon)
+		if err != nil {
+			return false, err
+		}
+		if !frameEquality {
+			return false, nil
+		}
+	}
+	return true, nil
+}
diff --git a/referenceframe/frame_system_test.go b/referenceframe/frame_system_test.go
index 95cadd1e475..793f396763e 100644
--- a/referenceframe/frame_system_test.go
+++ b/referenceframe/frame_system_test.go
@@ -4,7 +4,6 @@ import (
 	"encoding/json"
 	"math"
 	"os"
-	"sort"
 	"testing"
 
 	"github.com/golang/geo/r3"
@@ -440,15 +439,27 @@ func TestSerialization(t *testing.T) {
 	test.That(t, err, test.ShouldBeNil)
 	fs.AddFrame(blockFrame, model)
 
+	// Revolute joint around X axis
+	joint, err := NewRotationalFrame("rot", spatial.R4AA{RX: 1, RY: 0, RZ: 0}, Limit{Min: -math.Pi * 2, Max: math.Pi * 2})
+	test.That(t, err, test.ShouldBeNil)
+	fs.AddFrame(joint, fs.World())
+
+	// Translational frame
+	bc, err := spatial.NewBox(spatial.NewZeroPose(), r3.Vector{X: 1, Y: 1, Z: 1}, "")
+	test.That(t, err, test.ShouldBeNil)
+
+	// test creating a new translational frame with a geometry
+	prismatic, err := NewTranslationalFrameWithGeometry("pr", r3.Vector{X: 0, Y: 1, Z: 0}, Limit{Min: -30, Max: 30}, bc)
+	test.That(t, err, test.ShouldBeNil)
+	fs.AddFrame(prismatic, fs.World())
+
 	jsonData, err = json.Marshal(fs)
 	test.That(t, err, test.ShouldBeNil)
 
 	var fs2 FrameSystem
 	test.That(t, json.Unmarshal(jsonData, &fs2), test.ShouldBeNil)
 
-	frames1 := fs.FrameNames()
-	sort.Strings(frames1)
-	frames2 := fs2.FrameNames()
-	sort.Strings(frames2)
-	test.That(t, frames1, test.ShouldResemble, frames2)
+	equality, err := frameSystemsAlmostEqual(fs, &fs2, 1e-8)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, equality, test.ShouldBeTrue)
 }
diff --git a/referenceframe/frame_test.go b/referenceframe/frame_test.go
index 934ac4afa9d..8687d688d2e 100644
--- a/referenceframe/frame_test.go
+++ b/referenceframe/frame_test.go
@@ -15,8 +15,11 @@ import (
 	"go.viam.com/utils"
 
 	spatial "go.viam.com/rdk/spatialmath"
+	rdkutils "go.viam.com/rdk/utils"
 )
 
+const frameDifferenceEpsilon = 1e-8
+
 func TestStaticFrame(t *testing.T) {
 	// define a static transform
 	expPose := spatial.NewPose(r3.Vector{1, 2, 3}, &spatial.R4AA{math.Pi / 2, 0., 0., 1.})
@@ -280,3 +283,75 @@ func TestFrame(t *testing.T) {
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, sFrame, test.ShouldResemble, expStaticFrame)
 }
+
+func TestFrameToJSONAndBack(t *testing.T) {
+	// static frame
+	static, err := NewStaticFrame("foo", spatial.NewPose(r3.Vector{X: 1, Y: 2, Z: 3}, &spatial.R4AA{Theta: math.Pi / 2, RX: 4, RY: 5, RZ: 6}))
+	test.That(t, err, test.ShouldBeNil)
+
+	jsonData, err := frameToJSON(static)
+	test.That(t, err, test.ShouldBeNil)
+
+	static2, err := jsonToFrame(json.RawMessage(jsonData))
+	test.That(t, err, test.ShouldBeNil)
+
+	eq, err := framesAlmostEqual(static, static2, frameDifferenceEpsilon)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, eq, test.ShouldBeTrue)
+
+	staticFrame, ok := static.(*staticFrame)
+	test.That(t, ok, test.ShouldBeTrue)
+	tailGeoFrame := tailGeometryStaticFrame{staticFrame: staticFrame}
+
+	jsonData, err = frameToJSON(&tailGeoFrame)
+	test.That(t, err, test.ShouldBeNil)
+
+	tailGeoFrameParsed, err := jsonToFrame(json.RawMessage(jsonData))
+	test.That(t, err, test.ShouldBeNil)
+
+	eq, err = framesAlmostEqual(&tailGeoFrame, tailGeoFrameParsed, frameDifferenceEpsilon)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, eq, test.ShouldBeTrue)
+
+	// translational frame
+	tF, err := NewTranslationalFrame("foo", r3.Vector{X: 1, Y: 0, Z: 0}, Limit{1, 2})
+	test.That(t, err, test.ShouldBeNil)
+
+	jsonData, err = frameToJSON(tF)
+	test.That(t, err, test.ShouldBeNil)
+
+	tF2, err := jsonToFrame(json.RawMessage(jsonData))
+	test.That(t, err, test.ShouldBeNil)
+
+	eq, err = framesAlmostEqual(tF, tF2, frameDifferenceEpsilon)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, eq, test.ShouldBeTrue)
+
+	// rotational frame
+	rot, err := NewRotationalFrame("foo", spatial.R4AA{Theta: 3.7, RX: 2.1, RY: 3.1, RZ: 4.1}, Limit{5, 6})
+	test.That(t, err, test.ShouldBeNil)
+
+	jsonData, err = frameToJSON(rot)
+	test.That(t, err, test.ShouldBeNil)
+
+	rot2, err := jsonToFrame(json.RawMessage(jsonData))
+	test.That(t, err, test.ShouldBeNil)
+
+	eq, err = framesAlmostEqual(rot, rot2, frameDifferenceEpsilon)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, eq, test.ShouldBeTrue)
+
+	// SimpleModel
+	simpleModel, err := ParseModelJSONFile(rdkutils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "")
+	test.That(t, err, test.ShouldBeNil)
+
+	jsonData, err = frameToJSON(simpleModel)
+	test.That(t, err, test.ShouldBeNil)
+
+	simpleModel2, err := jsonToFrame(json.RawMessage(jsonData))
+	test.That(t, err, test.ShouldBeNil)
+
+	eq, err = framesAlmostEqual(simpleModel, simpleModel2, frameDifferenceEpsilon)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, eq, test.ShouldBeTrue)
+}
diff --git a/referenceframe/model.go b/referenceframe/model.go
index 23685bdcd66..2bcdbc10cc2 100644
--- a/referenceframe/model.go
+++ b/referenceframe/model.go
@@ -245,17 +245,50 @@ func (m *SimpleModel) DoF() []Limit {
 
 // MarshalJSON serializes a Model.
 func (m *SimpleModel) MarshalJSON() ([]byte, error) {
-	return json.Marshal(m.modelConfig)
+	type serialized struct {
+		Name   string           `json:"name"`
+		Model  *ModelConfigJSON `json:"model"`
+		Limits []Limit          `json:"limits"`
+	}
+	ser := serialized{
+		Name:   m.name,
+		Model:  m.modelConfig,
+		Limits: m.limits,
+	}
+	return json.Marshal(ser)
 }
 
 // UnmarshalJSON deserializes a Model.
 func (m *SimpleModel) UnmarshalJSON(data []byte) error {
-	var modelConfig ModelConfigJSON
-	if err := json.Unmarshal(data, &modelConfig); err != nil {
+	type serialized struct {
+		Name   string           `json:"name"`
+		Model  *ModelConfigJSON `json:"model"`
+		Limits []Limit          `json:"limits"`
+	}
+	var ser serialized
+	if err := json.Unmarshal(data, &ser); err != nil {
 		return err
 	}
-	m.baseFrame = &baseFrame{name: modelConfig.Name}
-	m.modelConfig = &modelConfig
+
+	frameName := ser.Name
+	if frameName == "" {
+		frameName = ser.Model.Name
+	}
+
+	if ser.Model != nil {
+		parsed, err := ser.Model.ParseConfig(ser.Model.Name)
+		if err != nil {
+			return err
+		}
+		newModel, ok := parsed.(*SimpleModel)
+		if !ok {
+			return fmt.Errorf("could not parse config for simple model, name: %v", ser.Name)
+		}
+		m.OrdTransforms = newModel.OrdTransforms
+	}
+	m.baseFrame = &baseFrame{name: frameName, limits: ser.Limits}
+	m.modelConfig = ser.Model
+
 	return nil
 }
 
diff --git a/referenceframe/model_json_test.go b/referenceframe/model_json_test.go
index 0bf889c0732..5ff3a16f9ad 100644
--- a/referenceframe/model_json_test.go
+++ b/referenceframe/model_json_test.go
@@ -1,6 +1,7 @@
 package referenceframe
 
 import (
+	"encoding/json"
 	"testing"
 
 	"go.viam.com/test"
@@ -42,13 +43,18 @@ func TestParseJSONFile(t *testing.T) {
 			model, err := ParseModelJSONFile(utils.ResolveFile(f), "")
 			test.That(t, err, test.ShouldBeNil)
 
-			data, err := model.MarshalJSON()
+			smodel, ok := model.(*SimpleModel)
+			test.That(t, ok, test.ShouldBeTrue)
+			data, err := json.Marshal(smodel.modelConfig)
 			test.That(t, err, test.ShouldBeNil)
 
 			model2, err := UnmarshalModelJSON(data, "")
 			test.That(t, err, test.ShouldBeNil)
 
-			data2, err := model2.MarshalJSON()
+			smodel2, ok := model2.(*SimpleModel)
+			test.That(t, ok, test.ShouldBeTrue)
+
+			data2, err := json.Marshal(smodel2.modelConfig)
 			test.That(t, err, test.ShouldBeNil)
 
 			test.That(t, data, test.ShouldResemble, data2)
diff --git a/referenceframe/register.go b/referenceframe/register.go
index 377dcaca657..288b7b4d9da 100644
--- a/referenceframe/register.go
+++ b/referenceframe/register.go
@@ -20,6 +20,9 @@ func init() {
 	if err := RegisterFrameImplementer((*SimpleModel)(nil), "model"); err != nil {
 		panic(err)
 	}
+	if err := RegisterFrameImplementer((*tailGeometryStaticFrame)(nil), "tail_geometry_static"); err != nil {
+		panic(err)
+	}
 }
 
 // RegisterFrameImplementer allows outside packages to register their implementations of the Frame
diff --git a/spatialmath/geometry.go b/spatialmath/geometry.go
index 2890b8fe43b..dce3feb2a6e 100644
--- a/spatialmath/geometry.go
+++ b/spatialmath/geometry.go
@@ -170,6 +170,12 @@ func (config *GeometryConfig) ToProtobuf() (*commonpb.Geometry, error) {
 
 // GeometriesAlmostEqual returns a bool describing if the two input Geometries are equal.
 func GeometriesAlmostEqual(a, b Geometry) bool {
+	if a == nil {
+		return b == nil
+	} else if b == nil {
+		return false
+	}
+
 	switch gType := a.(type) {
 	case *box:
 		return gType.almostEqual(b)
diff --git a/spatialmath/orientation.go b/spatialmath/orientation.go
index f6653fdfeee..7fae838d49b 100644
--- a/spatialmath/orientation.go
+++ b/spatialmath/orientation.go
@@ -32,6 +32,12 @@ func OrientationAlmostEqual(o1, o2 Orientation) bool {
 
 // OrientationAlmostEqualEps will return a bool describing whether 2 poses have approximately the same orientation.
 func OrientationAlmostEqualEps(o1, o2 Orientation, epsilon float64) bool {
+	if o1 == nil {
+		return o2 == nil
+	} else if o2 == nil {
+		return false
+	}
+
 	return QuatToR3AA(OrientationBetween(o1, o2).Quaternion()).Norm2() < epsilon
 }
 
diff --git a/spatialmath/pose.go b/spatialmath/pose.go
index 2eeb14d46e5..7d6fdc95ffb 100644
--- a/spatialmath/pose.go
+++ b/spatialmath/pose.go
@@ -176,6 +176,12 @@ func PoseAlmostCoincident(a, b Pose) bool {
 // PoseAlmostCoincidentEps will return a bool describing whether 2 poses approximately are at the same 3D coordinate location.
 // This uses a passed in epsilon value.
 func PoseAlmostCoincidentEps(a, b Pose, epsilon float64) bool {
+	if a == nil {
+		return b == nil
+	} else if b == nil {
+		return false
+	}
+
 	return R3VectorAlmostEqual(a.Point(), b.Point(), epsilon)
 }