changes

referenceframe/frame.go

@@ -666,14 +666,14 @@ func PoseToInputs(p spatial.Pose) []Input {
 	})
 }
 
-func framesAlmostEqual(frame1, frame2 Frame) bool {
+func framesAlmostEqual(frame1, frame2 Frame) (bool, error) {
 	switch {
 	case reflect.TypeOf(frame1) != reflect.TypeOf(frame2):
-		return false
+		return false, nil
 	case frame1.Name() != frame2.Name():
-		return false
+		return false, nil
 	case !limitsAlmostEqual(frame1.DoF(), frame2.DoF()):
-		return false
+		return false, nil
 	default:
 	}
 
@@ -682,36 +682,36 @@ func framesAlmostEqual(frame1, frame2 Frame) bool {
 		f2 := frame2.(*staticFrame)
 		switch {
 		case !spatial.PoseAlmostEqual(f1.transform, f2.transform):
-			return false
+			return false, nil
 		case f1.geometry == nil && f2.geometry == nil:
-			return true
+			return true, nil
 		case !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry):
-			return false
+			return false, nil
 		default:
 		}
 	case *rotationalFrame:
 		f2 := frame2.(*rotationalFrame)
 		if !spatial.R3VectorAlmostEqual(f1.rotAxis, f2.rotAxis, 1e-8) {
-			return false
+			return false, nil
 		}
 	case *translationalFrame:
 		f2 := frame2.(*translationalFrame)
 		switch {
 		case !spatial.R3VectorAlmostEqual(f1.transAxis, f2.transAxis, 1e-8):
-			return false
+			return false, nil
 		case f1.geometry == nil && f2.geometry == nil:
-			return true
+			return true, nil
 		case !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry):
-			return false
+			return false, nil
 		default:
 		}
 	case *tailGeometryStaticFrame:
 		f2 := frame2.(*tailGeometryStaticFrame)
 		switch {
 		case f1.staticFrame == nil:
-			return f2.staticFrame == nil
+			return f2.staticFrame == nil, nil
 		case f2.staticFrame == nil:
-			return f1.staticFrame == nil
+			return f1.staticFrame == nil, nil
 		default:
 			return framesAlmostEqual(f1.staticFrame, f2.staticFrame)
 		}
@@ -720,14 +720,20 @@ func framesAlmostEqual(frame1, frame2 Frame) bool {
 		ordTransforms1 := f1.OrdTransforms
 		ordTransforms2 := f2.OrdTransforms
 		if len(ordTransforms1) != len(ordTransforms2) {
-			return false
+			return false, nil
 		} else {
 			for i, f := range ordTransforms1 {
-				if !framesAlmostEqual(f, ordTransforms2[i]) {
-					return false
+				frameEquality, err := framesAlmostEqual(f, ordTransforms2[i])
+				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
+	return true, nil
 }

referenceframe/frame_system.go

@@ -756,33 +756,42 @@ func TopologicallySortParts(parts []*FrameSystemPart) ([]*FrameSystemPart, error
 	return topoSortedParts, nil
 }
 
-func frameSystemsAlmostEqual(fs1, fs2 *FrameSystem) bool {
+func frameSystemsAlmostEqual(fs1, fs2 *FrameSystem) (bool, error) {
 	if fs1.Name() != fs2.Name() {
-		return false
+		return false, nil
 	}
 
-	if !framesAlmostEqual(fs1.World(), fs2.World()) {
-		return false
+	worldFrameEquality, err := framesAlmostEqual(fs1.World(), fs2.World())
+	if err != nil {
+		return false, err
+	}
+	if !worldFrameEquality {
+		return false, nil
 	}
 
 	if !reflect.DeepEqual(fs1.parents, fs2.parents) {
-		return false
+		return false, nil
 	}
 
 	seenFrames := map[string]struct{}{}
 	for frameName, frame := range fs1.frames {
 		frame2, ok := fs2.frames[frameName]
 		if !ok {
-			return false
-		} else if !framesAlmostEqual(frame, frame2) {
-			return false
+			return false, nil
+		}
+		frameEquality, err := framesAlmostEqual(frame, frame2)
+		if err != nil {
+			return false, err
+		}
+		if !frameEquality {
+			return false, nil
 		}
 		seenFrames[frameName] = struct{}{}
 	}
 	for _, name := range fs2.FrameNames() {
 		if _, ok := seenFrames[name]; !ok {
-			return false
+			return false, nil
 		}
 	}
-	return true
+	return true, nil
 }

referenceframe/frame_system_test.go

@@ -459,5 +459,7 @@ func TestSerialization(t *testing.T) {
 	var fs2 FrameSystem
 	test.That(t, json.Unmarshal(jsonData, &fs2), test.ShouldBeNil)
 
-	test.That(t, frameSystemsAlmostEqual(fs, &fs2), test.ShouldBeTrue)
+	equality, err := frameSystemsAlmostEqual(fs, &fs2)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, equality, test.ShouldBeTrue)
 }

referenceframe/frame_test.go

@@ -293,7 +293,9 @@ func TestFrameToJSONAndBack(t *testing.T) {
 	static2, err := jsonToFrame(json.RawMessage(jsonData))
 	test.That(t, err, test.ShouldBeNil)
 
-	test.That(t, framesAlmostEqual(static, static2), test.ShouldBeTrue)
+	eq, err := framesAlmostEqual(static, static2)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, eq, test.ShouldBeTrue)
 
 	staticFrame, ok := static.(*staticFrame)
 	test.That(t, ok, test.ShouldBeTrue)
@@ -305,7 +307,9 @@ func TestFrameToJSONAndBack(t *testing.T) {
 	tailGeoFrameParsed, err := jsonToFrame(json.RawMessage(jsonData))
 	test.That(t, err, test.ShouldBeNil)
 
-	test.That(t, framesAlmostEqual(&tailGeoFrame, tailGeoFrameParsed), test.ShouldBeTrue)
+	eq, err = framesAlmostEqual(&tailGeoFrame, tailGeoFrameParsed)
+	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})
@@ -317,7 +321,9 @@ func TestFrameToJSONAndBack(t *testing.T) {
 	tF2, err := jsonToFrame(json.RawMessage(jsonData))
 	test.That(t, err, test.ShouldBeNil)
 
-	test.That(t, framesAlmostEqual(tF, tF2), test.ShouldBeTrue)
+	eq, err = framesAlmostEqual(tF, tF2)
+	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})
@@ -329,7 +335,9 @@ func TestFrameToJSONAndBack(t *testing.T) {
 	rot2, err := jsonToFrame(json.RawMessage(jsonData))
 	test.That(t, err, test.ShouldBeNil)
 
-	test.That(t, framesAlmostEqual(rot, rot2), test.ShouldBeTrue)
+	eq, err = framesAlmostEqual(rot, rot2)
+	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"), "")
@@ -341,5 +349,7 @@ func TestFrameToJSONAndBack(t *testing.T) {
 	simpleModel2, err := jsonToFrame(json.RawMessage(jsonData))
 	test.That(t, err, test.ShouldBeNil)
 
-	test.That(t, framesAlmostEqual(simpleModel, simpleModel2), test.ShouldBeTrue)
+	eq, err = framesAlmostEqual(simpleModel, simpleModel2)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, eq, test.ShouldBeTrue)
 }