[DO NOT MERGE] Fix frame system serialize main

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
+}

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
 }

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
+}

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)
 }

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)
+}