From bebe4f67dedb7aa240b4957b339068652d91b2bf Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Wed, 30 Jul 2025 19:38:59 -0400 Subject: [PATCH 01/16] fix bug in frame system serialization --- .../armplanning/data/plan_request_sample.json | 7 ++++-- referenceframe/frame_system.go | 7 +++--- referenceframe/frame_system_test.go | 22 +++++++++++++++++++ 3 files changed, 31 insertions(+), 5 deletions(-) diff --git a/motionplan/armplanning/data/plan_request_sample.json b/motionplan/armplanning/data/plan_request_sample.json index 42f1fc71e96..b405007769d 100644 --- a/motionplan/armplanning/data/plan_request_sample.json +++ b/motionplan/armplanning/data/plan_request_sample.json @@ -327,7 +327,10 @@ } } }, - "parents": null + "parents": { + "xArm6": "world", + "xArmVgripper": "xArm6" + } }, "goals": [ { @@ -428,7 +431,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_system.go b/referenceframe/frame_system.go index 009bf2fc224..454a7568d3a 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -458,9 +458,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) } diff --git a/referenceframe/frame_system_test.go b/referenceframe/frame_system_test.go index 95cadd1e475..f427580b780 100644 --- a/referenceframe/frame_system_test.go +++ b/referenceframe/frame_system_test.go @@ -451,4 +451,26 @@ func TestSerialization(t *testing.T) { frames2 := fs2.FrameNames() sort.Strings(frames2) test.That(t, frames1, test.ShouldResemble, frames2) + + for _, frameName := range frames1 { + frame1 := fs.Frame(frameName) + frame2 := fs2.Frame(frameName) + frame1JsonBytes, err := json.Marshal(frame1) + test.That(t, err, test.ShouldBeNil) + frame2JsonBytes, err := json.Marshal(frame2) + test.That(t, err, test.ShouldBeNil) + test.That(t, frame1JsonBytes, test.ShouldResemble, frame2JsonBytes) + + parentFrame1, err := fs.Parent(frame1) + test.That(t, err, test.ShouldBeNil) + parentFrame2, err := fs2.Parent(frame2) + test.That(t, err, test.ShouldBeNil) + test.That(t, parentFrame1, test.ShouldNotBeNil) + test.That(t, parentFrame2, test.ShouldNotBeNil) + parentFrame1JsonBytes, err := json.Marshal(parentFrame1) + test.That(t, err, test.ShouldBeNil) + parentFrame2JsonBytes, err := json.Marshal(parentFrame2) + test.That(t, err, test.ShouldBeNil) + test.That(t, parentFrame1JsonBytes, test.ShouldResemble, parentFrame2JsonBytes) + } } From 56d13bf86b64a2a31c33b21c019a2c01e01cdfe6 Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Thu, 31 Jul 2025 17:07:12 -0400 Subject: [PATCH 02/16] fixes, more tests incoming --- .../armplanning/data/plan_request_sample.json | 441 +++++++++--------- referenceframe/frame_json.go | 3 + referenceframe/frame_system.go | 4 +- referenceframe/frame_system_test.go | 22 +- referenceframe/model.go | 39 +- referenceframe/model_json_test.go | 10 +- referenceframe/register.go | 3 + 7 files changed, 273 insertions(+), 249 deletions(-) diff --git a/motionplan/armplanning/data/plan_request_sample.json b/motionplan/armplanning/data/plan_request_sample.json index b405007769d..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" } } }, diff --git a/referenceframe/frame_json.go b/referenceframe/frame_json.go index 067e78b3c0a..ea73782ac42 100644 --- a/referenceframe/frame_json.go +++ b/referenceframe/frame_json.go @@ -199,6 +199,9 @@ 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 { diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index 454a7568d3a..4c95977896e 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -589,7 +589,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 { @@ -600,7 +600,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 } diff --git a/referenceframe/frame_system_test.go b/referenceframe/frame_system_test.go index f427580b780..ad02aadf924 100644 --- a/referenceframe/frame_system_test.go +++ b/referenceframe/frame_system_test.go @@ -452,25 +452,5 @@ func TestSerialization(t *testing.T) { sort.Strings(frames2) test.That(t, frames1, test.ShouldResemble, frames2) - for _, frameName := range frames1 { - frame1 := fs.Frame(frameName) - frame2 := fs2.Frame(frameName) - frame1JsonBytes, err := json.Marshal(frame1) - test.That(t, err, test.ShouldBeNil) - frame2JsonBytes, err := json.Marshal(frame2) - test.That(t, err, test.ShouldBeNil) - test.That(t, frame1JsonBytes, test.ShouldResemble, frame2JsonBytes) - - parentFrame1, err := fs.Parent(frame1) - test.That(t, err, test.ShouldBeNil) - parentFrame2, err := fs2.Parent(frame2) - test.That(t, err, test.ShouldBeNil) - test.That(t, parentFrame1, test.ShouldNotBeNil) - test.That(t, parentFrame2, test.ShouldNotBeNil) - parentFrame1JsonBytes, err := json.Marshal(parentFrame1) - test.That(t, err, test.ShouldBeNil) - parentFrame2JsonBytes, err := json.Marshal(parentFrame2) - test.That(t, err, test.ShouldBeNil) - test.That(t, parentFrame1JsonBytes, test.ShouldResemble, parentFrame2JsonBytes) - } + test.That(t, fs, test.ShouldResemble, &fs2) } diff --git a/referenceframe/model.go b/referenceframe/model.go index 23685bdcd66..665cf413df4 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -245,17 +245,46 @@ 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"` + } + ser := serialized{ + Name: m.name, + Model: m.modelConfig, + } + 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"` + } + var ser serialized + if err := json.Unmarshal(data, &ser); err != nil { + return err + } + + frameName := ser.Name + if frameName == "" { + frameName = ser.Model.Name + } + m.baseFrame = &baseFrame{name: frameName} + m.modelConfig = ser.Model + if ser.Model == nil { + return fmt.Errorf("could not unmarshal simple model frame json, 'model' key missing") + } + parsed, err := ser.Model.ParseConfig(m.modelConfig.Name) + if err != nil { return err } - m.baseFrame = &baseFrame{name: modelConfig.Name} - m.modelConfig = &modelConfig + newModel, ok := parsed.(*SimpleModel) + if !ok { + return fmt.Errorf("could not parse config for simple model, name: %v", ser.Name) + } + m.OrdTransforms = newModel.OrdTransforms 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 From 28d75294e684536ec5bc9703d624e806f8545002 Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Thu, 31 Jul 2025 18:32:46 -0400 Subject: [PATCH 03/16] missed a spot --- referenceframe/model.go | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/referenceframe/model.go b/referenceframe/model.go index 665cf413df4..9c4e796a96f 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -246,12 +246,14 @@ func (m *SimpleModel) DoF() []Limit { // MarshalJSON serializes a Model. func (m *SimpleModel) MarshalJSON() ([]byte, error) { type serialized struct { - Name string `json:"name"` - Model *ModelConfigJSON `json:"model"` + Name string `json:"name"` + Model *ModelConfigJSON `json:"model"` + Limits []Limit `json:"limits"` } ser := serialized{ - Name: m.name, - Model: m.modelConfig, + Name: m.name, + Model: m.modelConfig, + Limits: m.limits, } return json.Marshal(ser) } @@ -259,8 +261,9 @@ func (m *SimpleModel) MarshalJSON() ([]byte, error) { // UnmarshalJSON deserializes a Model. func (m *SimpleModel) UnmarshalJSON(data []byte) error { type serialized struct { - Name string `json:"name"` - Model *ModelConfigJSON `json:"model"` + Name string `json:"name"` + Model *ModelConfigJSON `json:"model"` + Limits []Limit `json:"limits"` } var ser serialized if err := json.Unmarshal(data, &ser); err != nil { @@ -271,7 +274,7 @@ func (m *SimpleModel) UnmarshalJSON(data []byte) error { if frameName == "" { frameName = ser.Model.Name } - m.baseFrame = &baseFrame{name: frameName} + m.baseFrame = &baseFrame{name: frameName, limits: ser.Limits} m.modelConfig = ser.Model if ser.Model == nil { return fmt.Errorf("could not unmarshal simple model frame json, 'model' key missing") From 81104f6ff8c77816807238922983e91f6b0d1bc9 Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Fri, 1 Aug 2025 19:12:31 -0400 Subject: [PATCH 04/16] add other frame types to test + fixes --- referenceframe/frame.go | 3 ++- referenceframe/frame_system_test.go | 14 ++++++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 995a1a1f28f..c1b2f408825 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -501,7 +501,8 @@ 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 diff --git a/referenceframe/frame_system_test.go b/referenceframe/frame_system_test.go index ad02aadf924..d6391784272 100644 --- a/referenceframe/frame_system_test.go +++ b/referenceframe/frame_system_test.go @@ -440,6 +440,20 @@ 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) From 1aa713be831917738b9020ef3b2fe7a21540df1b Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Fri, 1 Aug 2025 19:22:56 -0400 Subject: [PATCH 05/16] oops --- referenceframe/frame.go | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index c1b2f408825..34e7a5768db 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -502,7 +502,8 @@ func (rf *rotationalFrame) UnmarshalJSON(data []byte) error { } rf.baseFrame = &baseFrame{name: cfg.ID, limits: []Limit{ - {Min: utils.DegToRad(cfg.Min), Max: utils.DegToRad(cfg.Max)}}} + {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 From 2c1bbf7217864e183e2815f7d0cb42e450d19c14 Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Mon, 4 Aug 2025 11:45:29 -0400 Subject: [PATCH 06/16] fix not allowing lack of model config --- referenceframe/model.go | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/referenceframe/model.go b/referenceframe/model.go index 9c4e796a96f..2bcdbc10cc2 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -274,20 +274,21 @@ func (m *SimpleModel) UnmarshalJSON(data []byte) error { 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 - if ser.Model == nil { - return fmt.Errorf("could not unmarshal simple model frame json, 'model' key missing") - } - parsed, err := ser.Model.ParseConfig(m.modelConfig.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 + return nil } From ead54235071acfea703d1295c07a211de4c130b7 Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Mon, 4 Aug 2025 16:43:42 -0400 Subject: [PATCH 07/16] add roundtrip tests for all frame implementers --- referenceframe/frame.go | 11 ++++++ referenceframe/frame_json.go | 4 +-- referenceframe/frame_test.go | 65 ++++++++++++++++++++++++++++++++++++ 3 files changed, 78 insertions(+), 2 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 34e7a5768db..c250b4687f7 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -193,6 +193,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 diff --git a/referenceframe/frame_json.go b/referenceframe/frame_json.go index ea73782ac42..372f13cb0f1 100644 --- a/referenceframe/frame_json.go +++ b/referenceframe/frame_json.go @@ -208,10 +208,10 @@ func jsonToFrame(data json.RawMessage) (Frame, error) { 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() + if err := json.Unmarshal(sF["frame"], frame); err != nil { return nil, err } - frame := frameZeroStruct.Addr().Interface() return utils.AssertType[Frame](frame) } diff --git a/referenceframe/frame_test.go b/referenceframe/frame_test.go index 934ac4afa9d..3b7bb8a7b6f 100644 --- a/referenceframe/frame_test.go +++ b/referenceframe/frame_test.go @@ -11,6 +11,7 @@ import ( "github.com/golang/geo/r3" "github.com/pkg/errors" pb "go.viam.com/api/component/arm/v1" + rdkutils "go.viam.com/rdk/utils" "go.viam.com/test" "go.viam.com/utils" @@ -280,3 +281,67 @@ 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) + + test.That(t, static, test.ShouldResemble, static2) + + staticFrame, ok := static.(*staticFrame) + test.That(t, ok, test.ShouldBeTrue) + tailGeoFrame := tailGeometryStaticFrame{staticFrame: staticFrame} + + jsonData, err = frameToJSON(&tailGeoFrame) + test.That(t, err, test.ShouldBeNil) + + tailGeoFrame2, err := jsonToFrame(json.RawMessage(jsonData)) + test.That(t, err, test.ShouldBeNil) + + test.That(t, &tailGeoFrame, test.ShouldResemble, tailGeoFrame2) + + // 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) + + test.That(t, tF, test.ShouldResemble, tF2) + + // 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) + + test.That(t, rot, test.ShouldResemble, rot2) + + // SimpleModel + simpleModel, err := ParseModelJSONFile(rdkutils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "") + test.That(t, err, test.ShouldBeNil) + // simpleModel, ok := m.(*SimpleModel) + // test.That(t, ok, test.ShouldBeTrue) + + jsonData, err = frameToJSON(simpleModel) + test.That(t, err, test.ShouldBeNil) + + simpleModel2, err := jsonToFrame(json.RawMessage(jsonData)) + test.That(t, err, test.ShouldBeNil) + + test.That(t, simpleModel, test.ShouldResemble, simpleModel2) +} From cfb1727f96456a8725fc2d2e3bf657e7da1d406f Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Mon, 4 Aug 2025 17:01:25 -0400 Subject: [PATCH 08/16] oops --- referenceframe/frame_test.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/referenceframe/frame_test.go b/referenceframe/frame_test.go index 3b7bb8a7b6f..f03a418f32c 100644 --- a/referenceframe/frame_test.go +++ b/referenceframe/frame_test.go @@ -11,11 +11,11 @@ import ( "github.com/golang/geo/r3" "github.com/pkg/errors" pb "go.viam.com/api/component/arm/v1" - rdkutils "go.viam.com/rdk/utils" "go.viam.com/test" "go.viam.com/utils" spatial "go.viam.com/rdk/spatialmath" + rdkutils "go.viam.com/rdk/utils" ) func TestStaticFrame(t *testing.T) { From bf376b05c49690465f3acbd8d8f346035cee4bbd Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Mon, 4 Aug 2025 18:10:50 -0400 Subject: [PATCH 09/16] use explicit comparisons over ShouldResemble --- referenceframe/frame_test.go | 37 +++++++++++++++++++++++++++++------- 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/referenceframe/frame_test.go b/referenceframe/frame_test.go index f03a418f32c..15bfcccb9cd 100644 --- a/referenceframe/frame_test.go +++ b/referenceframe/frame_test.go @@ -293,7 +293,15 @@ func TestFrameToJSONAndBack(t *testing.T) { static2, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - test.That(t, static, test.ShouldResemble, static2) + test.That(t, static.Name(), test.ShouldResemble, static2.Name()) + + staticF, ok := static.(*staticFrame) + test.That(t, ok, test.ShouldBeTrue) + + staticF2, ok := static2.(*staticFrame) + test.That(t, ok, test.ShouldBeTrue) + + test.That(t, spatial.PoseAlmostCoincident(staticF.transform, staticF2.transform), test.ShouldBeTrue) staticFrame, ok := static.(*staticFrame) test.That(t, ok, test.ShouldBeTrue) @@ -302,10 +310,13 @@ func TestFrameToJSONAndBack(t *testing.T) { jsonData, err = frameToJSON(&tailGeoFrame) test.That(t, err, test.ShouldBeNil) - tailGeoFrame2, err := jsonToFrame(json.RawMessage(jsonData)) + tailGeoFrameParsed, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) + tailGeoFrame2, ok := tailGeoFrameParsed.(*tailGeometryStaticFrame) + test.That(t, ok, test.ShouldBeTrue) - test.That(t, &tailGeoFrame, test.ShouldResemble, tailGeoFrame2) + test.That(t, tailGeoFrame.Name(), test.ShouldResemble, tailGeoFrame2.Name()) + test.That(t, spatial.PoseAlmostEqual(tailGeoFrame.transform, tailGeoFrame2.transform), test.ShouldBeTrue) // translational frame tF, err := NewTranslationalFrame("foo", r3.Vector{X: 1, Y: 0, Z: 0}, Limit{1, 2}) @@ -317,7 +328,14 @@ func TestFrameToJSONAndBack(t *testing.T) { tF2, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - test.That(t, tF, test.ShouldResemble, tF2) + tFrame1, ok := tF.(*translationalFrame) + test.That(t, ok, test.ShouldBeTrue) + + tFrame2, ok := tF2.(*translationalFrame) + test.That(t, ok, test.ShouldBeTrue) + + test.That(t, tFrame1.Name(), test.ShouldResemble, tFrame2.Name()) + test.That(t, spatial.R3VectorAlmostEqual(tFrame1.transAxis, tFrame2.transAxis, 1e-8), 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,13 +347,18 @@ func TestFrameToJSONAndBack(t *testing.T) { rot2, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - test.That(t, rot, test.ShouldResemble, rot2) + rotF, ok := rot.(*rotationalFrame) + test.That(t, ok, test.ShouldBeTrue) + + rotF2, ok := rot2.(*rotationalFrame) + test.That(t, ok, test.ShouldBeTrue) + + test.That(t, rotF.Name(), test.ShouldResemble, rotF2.Name()) + test.That(t, spatial.R3VectorAlmostEqual(rotF.rotAxis, rotF2.rotAxis, 1e-8), test.ShouldBeTrue) // SimpleModel simpleModel, err := ParseModelJSONFile(rdkutils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) - // simpleModel, ok := m.(*SimpleModel) - // test.That(t, ok, test.ShouldBeTrue) jsonData, err = frameToJSON(simpleModel) test.That(t, err, test.ShouldBeNil) From f5af70db5e2c20cc5c35bff01a0a0a874b5798fd Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Mon, 4 Aug 2025 19:50:53 -0400 Subject: [PATCH 10/16] implement better equality checks --- referenceframe/frame.go | 81 +++++++++++++++++++++++++++++ referenceframe/frame_system.go | 32 ++++++++++++ referenceframe/frame_system_test.go | 9 +--- referenceframe/frame_test.go | 35 ++----------- 4 files changed, 119 insertions(+), 38 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index c250b4687f7..8813ca05e0d 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 *Limit, limit2 *Limit) bool { + if math.Abs(limit1.Max-limit2.Max) > 1e-8 { + return false + } else if math.Abs(limit1.Min-limit2.Min) > 1e-8 { + 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 []Limit, limits2 []Limit) bool { + if len(limits1) != len(limits2) { + return false + } + for i, limit := range limits1 { + if !limitAlmostEqual(&limit, &limits2[i]) { + return false + } + } + return true +} + // Frame represents a reference frame, e.g. an arm, a joint, a gripper, a board, etc. type Frame interface { Limited @@ -643,3 +665,62 @@ func PoseToInputs(p spatial.Pose) []Input { p.Orientation().OrientationVectorRadians().Theta, }) } + +func framesAlmostEqual(frame1 Frame, frame2 Frame) bool { + if reflect.TypeOf(frame1) != reflect.TypeOf(frame2) { + return false + } else if frame1.Name() != frame2.Name() { + return false + } else if !limitsAlmostEqual(frame1.DoF(), frame2.DoF()) { + return false + } + + switch f1 := frame1.(type) { + case *staticFrame: + f2 := frame2.(*staticFrame) + if !spatial.PoseAlmostEqual(f1.transform, f2.transform) { + return false + } else if f1.geometry == nil && f2.geometry == nil { + return true + } else if !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry) { + return false + } + case *rotationalFrame: + f2 := frame2.(*rotationalFrame) + if !spatial.R3VectorAlmostEqual(f1.rotAxis, f2.rotAxis, 1e-8) { + return false + } + case *translationalFrame: + f2 := frame2.(*translationalFrame) + if !spatial.R3VectorAlmostEqual(f1.transAxis, f2.transAxis, 1e-8) { + return false + } else if f1.geometry == nil && f2.geometry == nil { + return true + } else if !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry) { + return false + } + case *tailGeometryStaticFrame: + f2 := frame2.(*tailGeometryStaticFrame) + if f1.staticFrame == nil { + return f2.staticFrame == nil + } else if f2.staticFrame == nil { + return f1.staticFrame == nil + } else { + return framesAlmostEqual(f1.staticFrame, f2.staticFrame) + } + case *SimpleModel: + f2 := frame2.(*SimpleModel) + ordTransforms1 := f1.OrdTransforms + ordTransforms2 := f2.OrdTransforms + if len(ordTransforms1) != len(ordTransforms2) { + return false + } else { + for i, f := range ordTransforms1 { + if !framesAlmostEqual(f, ordTransforms2[i]) { + return false + } + } + } + } + return true +} diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index 4c95977896e..89dc8768037 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" @@ -754,3 +755,34 @@ func TopologicallySortParts(parts []*FrameSystemPart) ([]*FrameSystemPart, error } return topoSortedParts, nil } + +func frameSystemsAlmostEqual(fs1, fs2 *FrameSystem) bool { + if fs1.Name() != fs2.Name() { + return false + } + + if !framesAlmostEqual(fs1.World(), fs2.World()) { + return false + } + + if !reflect.DeepEqual(fs1.parents, fs2.parents) { + return false + } + + 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 + } + seenFrames[frameName] = struct{}{} + } + for _, name := range fs2.FrameNames() { + if _, ok := seenFrames[name]; !ok { + return false + } + } + return true +} diff --git a/referenceframe/frame_system_test.go b/referenceframe/frame_system_test.go index d6391784272..da5a01c584b 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" @@ -460,11 +459,5 @@ func TestSerialization(t *testing.T) { 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) - - test.That(t, fs, test.ShouldResemble, &fs2) + test.That(t, frameSystemsAlmostEqual(fs, &fs2), test.ShouldBeTrue) } diff --git a/referenceframe/frame_test.go b/referenceframe/frame_test.go index 15bfcccb9cd..d7ac14730f5 100644 --- a/referenceframe/frame_test.go +++ b/referenceframe/frame_test.go @@ -293,15 +293,7 @@ func TestFrameToJSONAndBack(t *testing.T) { static2, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - test.That(t, static.Name(), test.ShouldResemble, static2.Name()) - - staticF, ok := static.(*staticFrame) - test.That(t, ok, test.ShouldBeTrue) - - staticF2, ok := static2.(*staticFrame) - test.That(t, ok, test.ShouldBeTrue) - - test.That(t, spatial.PoseAlmostCoincident(staticF.transform, staticF2.transform), test.ShouldBeTrue) + test.That(t, framesAlmostEqual(static, static2), test.ShouldBeTrue) staticFrame, ok := static.(*staticFrame) test.That(t, ok, test.ShouldBeTrue) @@ -312,11 +304,8 @@ func TestFrameToJSONAndBack(t *testing.T) { tailGeoFrameParsed, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - tailGeoFrame2, ok := tailGeoFrameParsed.(*tailGeometryStaticFrame) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, tailGeoFrame.Name(), test.ShouldResemble, tailGeoFrame2.Name()) - test.That(t, spatial.PoseAlmostEqual(tailGeoFrame.transform, tailGeoFrame2.transform), test.ShouldBeTrue) + test.That(t, framesAlmostEqual(&tailGeoFrame, tailGeoFrameParsed), test.ShouldBeTrue) // translational frame tF, err := NewTranslationalFrame("foo", r3.Vector{X: 1, Y: 0, Z: 0}, Limit{1, 2}) @@ -328,14 +317,7 @@ func TestFrameToJSONAndBack(t *testing.T) { tF2, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - tFrame1, ok := tF.(*translationalFrame) - test.That(t, ok, test.ShouldBeTrue) - - tFrame2, ok := tF2.(*translationalFrame) - test.That(t, ok, test.ShouldBeTrue) - - test.That(t, tFrame1.Name(), test.ShouldResemble, tFrame2.Name()) - test.That(t, spatial.R3VectorAlmostEqual(tFrame1.transAxis, tFrame2.transAxis, 1e-8), test.ShouldBeTrue) + test.That(t, framesAlmostEqual(tF, tF2), 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}) @@ -347,14 +329,7 @@ func TestFrameToJSONAndBack(t *testing.T) { rot2, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - rotF, ok := rot.(*rotationalFrame) - test.That(t, ok, test.ShouldBeTrue) - - rotF2, ok := rot2.(*rotationalFrame) - test.That(t, ok, test.ShouldBeTrue) - - test.That(t, rotF.Name(), test.ShouldResemble, rotF2.Name()) - test.That(t, spatial.R3VectorAlmostEqual(rotF.rotAxis, rotF2.rotAxis, 1e-8), test.ShouldBeTrue) + test.That(t, framesAlmostEqual(rot, rot2), test.ShouldBeTrue) // SimpleModel simpleModel, err := ParseModelJSONFile(rdkutils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "") @@ -366,5 +341,5 @@ func TestFrameToJSONAndBack(t *testing.T) { simpleModel2, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - test.That(t, simpleModel, test.ShouldResemble, simpleModel2) + test.That(t, framesAlmostEqual(simpleModel, simpleModel2), test.ShouldBeTrue) } From 6b49d8d00be6807cf15f89d02d4e13e5814820a3 Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Mon, 4 Aug 2025 20:35:47 -0400 Subject: [PATCH 11/16] lint --- referenceframe/frame.go | 31 +++++++++++++++++++------------ 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 8813ca05e0d..c31039e09cd 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -667,23 +667,27 @@ func PoseToInputs(p spatial.Pose) []Input { } func framesAlmostEqual(frame1 Frame, frame2 Frame) bool { - if reflect.TypeOf(frame1) != reflect.TypeOf(frame2) { + switch { + case reflect.TypeOf(frame1) != reflect.TypeOf(frame2): return false - } else if frame1.Name() != frame2.Name() { + case frame1.Name() != frame2.Name(): return false - } else if !limitsAlmostEqual(frame1.DoF(), frame2.DoF()) { + case !limitsAlmostEqual(frame1.DoF(), frame2.DoF()): return false + default: } switch f1 := frame1.(type) { case *staticFrame: f2 := frame2.(*staticFrame) - if !spatial.PoseAlmostEqual(f1.transform, f2.transform) { + switch { + case !spatial.PoseAlmostEqual(f1.transform, f2.transform): return false - } else if f1.geometry == nil && f2.geometry == nil { + case f1.geometry == nil && f2.geometry == nil: return true - } else if !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry) { + case !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry): return false + default: } case *rotationalFrame: f2 := frame2.(*rotationalFrame) @@ -692,20 +696,23 @@ func framesAlmostEqual(frame1 Frame, frame2 Frame) bool { } case *translationalFrame: f2 := frame2.(*translationalFrame) - if !spatial.R3VectorAlmostEqual(f1.transAxis, f2.transAxis, 1e-8) { + switch { + case !spatial.R3VectorAlmostEqual(f1.transAxis, f2.transAxis, 1e-8): return false - } else if f1.geometry == nil && f2.geometry == nil { + case f1.geometry == nil && f2.geometry == nil: return true - } else if !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry) { + case !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry): return false + default: } case *tailGeometryStaticFrame: f2 := frame2.(*tailGeometryStaticFrame) - if f1.staticFrame == nil { + switch { + case f1.staticFrame == nil: return f2.staticFrame == nil - } else if f2.staticFrame == nil { + case f2.staticFrame == nil: return f1.staticFrame == nil - } else { + default: return framesAlmostEqual(f1.staticFrame, f2.staticFrame) } case *SimpleModel: From e4c8551f1e57a1e9a237654c3d39dcd68f40da39 Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Mon, 4 Aug 2025 22:35:02 -0400 Subject: [PATCH 12/16] some missed cleanup --- referenceframe/frame.go | 6 +++--- referenceframe/frame_json.go | 6 +++++- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index c31039e09cd..a1f730a08df 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -30,7 +30,7 @@ type Limit struct { Max float64 } -func limitAlmostEqual(limit1 *Limit, limit2 *Limit) bool { +func limitAlmostEqual(limit1, limit2 *Limit) bool { if math.Abs(limit1.Max-limit2.Max) > 1e-8 { return false } else if math.Abs(limit1.Min-limit2.Min) > 1e-8 { @@ -103,7 +103,7 @@ type Limited interface { DoF() []Limit } -func limitsAlmostEqual(limits1 []Limit, limits2 []Limit) bool { +func limitsAlmostEqual(limits1, limits2 []Limit) bool { if len(limits1) != len(limits2) { return false } @@ -666,7 +666,7 @@ func PoseToInputs(p spatial.Pose) []Input { }) } -func framesAlmostEqual(frame1 Frame, frame2 Frame) bool { +func framesAlmostEqual(frame1, frame2 Frame) bool { switch { case reflect.TypeOf(frame1) != reflect.TypeOf(frame2): return false diff --git a/referenceframe/frame_json.go b/referenceframe/frame_json.go index 372f13cb0f1..5789858001f 100644 --- a/referenceframe/frame_json.go +++ b/referenceframe/frame_json.go @@ -209,9 +209,13 @@ func jsonToFrame(data json.RawMessage) (Frame, error) { } frameZeroStruct := reflect.New(implementer).Elem() 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 } - return utils.AssertType[Frame](frame) + return frameI, nil } From 8cbcb14cc9fea4f29e0d1dea06d19ae8593539bb Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Tue, 5 Aug 2025 10:12:43 -0400 Subject: [PATCH 13/16] changes --- referenceframe/frame.go | 40 +++++++++++++++++------------ referenceframe/frame_system.go | 29 +++++++++++++-------- referenceframe/frame_system_test.go | 4 ++- referenceframe/frame_test.go | 20 +++++++++++---- 4 files changed, 60 insertions(+), 33 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index a1f730a08df..32bfff00da0 100644 --- a/referenceframe/frame.go +++ b/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 } diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index 89dc8768037..bdb39a85171 100644 --- a/referenceframe/frame_system.go +++ b/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 } diff --git a/referenceframe/frame_system_test.go b/referenceframe/frame_system_test.go index da5a01c584b..29bb71d6064 100644 --- a/referenceframe/frame_system_test.go +++ b/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) } diff --git a/referenceframe/frame_test.go b/referenceframe/frame_test.go index d7ac14730f5..30518c7dee8 100644 --- a/referenceframe/frame_test.go +++ b/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) } From 0c4145dd3417ace3ff1be50451c202398ca034f0 Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Tue, 5 Aug 2025 10:35:14 -0400 Subject: [PATCH 14/16] add configurable epsilon --- referenceframe/frame.go | 30 ++++++++++++++++++----------- referenceframe/frame_system.go | 6 +++--- referenceframe/frame_system_test.go | 2 +- referenceframe/frame_test.go | 12 +++++++----- 4 files changed, 30 insertions(+), 20 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 32bfff00da0..69aa3a46188 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -30,10 +30,10 @@ type Limit struct { Max float64 } -func limitAlmostEqual(limit1, limit2 *Limit) bool { - if math.Abs(limit1.Max-limit2.Max) > 1e-8 { +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) > 1e-8 { + } else if math.Abs(limit1.Min-limit2.Min) > epsilon { return false } return true @@ -103,12 +103,12 @@ type Limited interface { DoF() []Limit } -func limitsAlmostEqual(limits1, limits2 []Limit) bool { +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]) { + if !limitAlmostEqual(&limit, &limits2[i], epsilon) { return false } } @@ -666,13 +666,21 @@ func PoseToInputs(p spatial.Pose) []Input { }) } -func framesAlmostEqual(frame1, frame2 Frame) (bool, error) { +// 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()): + case !limitsAlmostEqual(frame1.DoF(), frame2.DoF(), epsilon): return false, nil default: } @@ -691,13 +699,13 @@ func framesAlmostEqual(frame1, frame2 Frame) (bool, error) { } case *rotationalFrame: f2 := frame2.(*rotationalFrame) - if !spatial.R3VectorAlmostEqual(f1.rotAxis, f2.rotAxis, 1e-8) { + if !spatial.R3VectorAlmostEqual(f1.rotAxis, f2.rotAxis, epsilon) { return false, nil } case *translationalFrame: f2 := frame2.(*translationalFrame) switch { - case !spatial.R3VectorAlmostEqual(f1.transAxis, f2.transAxis, 1e-8): + case !spatial.R3VectorAlmostEqual(f1.transAxis, f2.transAxis, epsilon): return false, nil case f1.geometry == nil && f2.geometry == nil: return true, nil @@ -713,7 +721,7 @@ func framesAlmostEqual(frame1, frame2 Frame) (bool, error) { case f2.staticFrame == nil: return f1.staticFrame == nil, nil default: - return framesAlmostEqual(f1.staticFrame, f2.staticFrame) + return framesAlmostEqual(f1.staticFrame, f2.staticFrame, epsilon) } case *SimpleModel: f2 := frame2.(*SimpleModel) @@ -723,7 +731,7 @@ func framesAlmostEqual(frame1, frame2 Frame) (bool, error) { return false, nil } else { for i, f := range ordTransforms1 { - frameEquality, err := framesAlmostEqual(f, ordTransforms2[i]) + frameEquality, err := framesAlmostEqual(f, ordTransforms2[i], epsilon) if err != nil { return false, err } diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index bdb39a85171..7ba3c0e0c67 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -756,12 +756,12 @@ func TopologicallySortParts(parts []*FrameSystemPart) ([]*FrameSystemPart, error return topoSortedParts, nil } -func frameSystemsAlmostEqual(fs1, fs2 *FrameSystem) (bool, error) { +func frameSystemsAlmostEqual(fs1, fs2 *FrameSystem, epsilon float64) (bool, error) { if fs1.Name() != fs2.Name() { return false, nil } - worldFrameEquality, err := framesAlmostEqual(fs1.World(), fs2.World()) + worldFrameEquality, err := framesAlmostEqual(fs1.World(), fs2.World(), epsilon) if err != nil { return false, err } @@ -779,7 +779,7 @@ func frameSystemsAlmostEqual(fs1, fs2 *FrameSystem) (bool, error) { if !ok { return false, nil } - frameEquality, err := framesAlmostEqual(frame, frame2) + frameEquality, err := framesAlmostEqual(frame, frame2, epsilon) if err != nil { return false, err } diff --git a/referenceframe/frame_system_test.go b/referenceframe/frame_system_test.go index 29bb71d6064..793f396763e 100644 --- a/referenceframe/frame_system_test.go +++ b/referenceframe/frame_system_test.go @@ -459,7 +459,7 @@ func TestSerialization(t *testing.T) { var fs2 FrameSystem test.That(t, json.Unmarshal(jsonData, &fs2), test.ShouldBeNil) - equality, err := frameSystemsAlmostEqual(fs, &fs2) + 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 30518c7dee8..8687d688d2e 100644 --- a/referenceframe/frame_test.go +++ b/referenceframe/frame_test.go @@ -18,6 +18,8 @@ import ( 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.}) @@ -293,7 +295,7 @@ func TestFrameToJSONAndBack(t *testing.T) { static2, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - eq, err := framesAlmostEqual(static, static2) + eq, err := framesAlmostEqual(static, static2, frameDifferenceEpsilon) test.That(t, err, test.ShouldBeNil) test.That(t, eq, test.ShouldBeTrue) @@ -307,7 +309,7 @@ func TestFrameToJSONAndBack(t *testing.T) { tailGeoFrameParsed, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - eq, err = framesAlmostEqual(&tailGeoFrame, tailGeoFrameParsed) + eq, err = framesAlmostEqual(&tailGeoFrame, tailGeoFrameParsed, frameDifferenceEpsilon) test.That(t, err, test.ShouldBeNil) test.That(t, eq, test.ShouldBeTrue) @@ -321,7 +323,7 @@ func TestFrameToJSONAndBack(t *testing.T) { tF2, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - eq, err = framesAlmostEqual(tF, tF2) + eq, err = framesAlmostEqual(tF, tF2, frameDifferenceEpsilon) test.That(t, err, test.ShouldBeNil) test.That(t, eq, test.ShouldBeTrue) @@ -335,7 +337,7 @@ func TestFrameToJSONAndBack(t *testing.T) { rot2, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - eq, err = framesAlmostEqual(rot, rot2) + eq, err = framesAlmostEqual(rot, rot2, frameDifferenceEpsilon) test.That(t, err, test.ShouldBeNil) test.That(t, eq, test.ShouldBeTrue) @@ -349,7 +351,7 @@ func TestFrameToJSONAndBack(t *testing.T) { simpleModel2, err := jsonToFrame(json.RawMessage(jsonData)) test.That(t, err, test.ShouldBeNil) - eq, err = framesAlmostEqual(simpleModel, simpleModel2) + eq, err = framesAlmostEqual(simpleModel, simpleModel2, frameDifferenceEpsilon) test.That(t, err, test.ShouldBeNil) test.That(t, eq, test.ShouldBeTrue) } From 6c24169e77b832cd4a65d59cbc3cc3ab9e47ce11 Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Tue, 5 Aug 2025 10:52:18 -0400 Subject: [PATCH 15/16] pass Limits by value --- referenceframe/frame.go | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 69aa3a46188..362f206e9d7 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -30,7 +30,7 @@ type Limit struct { Max float64 } -func limitAlmostEqual(limit1, limit2 *Limit, epsilon float64) bool { +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 { @@ -108,7 +108,7 @@ func limitsAlmostEqual(limits1, limits2 []Limit, epsilon float64) bool { return false } for i, limit := range limits1 { - if !limitAlmostEqual(&limit, &limits2[i], epsilon) { + if !limitAlmostEqual(limit, limits2[i], epsilon) { return false } } @@ -673,7 +673,7 @@ func PoseToInputs(p spatial.Pose) []Input { // 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 +// this function should be changed accordingly. func framesAlmostEqual(frame1, frame2 Frame, epsilon float64) (bool, error) { switch { case reflect.TypeOf(frame1) != reflect.TypeOf(frame2): From 49b4f2596bfe09a428a096a9f33d565ed232eddc Mon Sep 17 00:00:00 2001 From: Gautham Varadarajan Date: Tue, 5 Aug 2025 13:22:18 -0400 Subject: [PATCH 16/16] implement nil checks --- referenceframe/frame.go | 10 ++++++---- referenceframe/frame_system.go | 11 ++++------- spatialmath/geometry.go | 6 ++++++ spatialmath/orientation.go | 6 ++++++ spatialmath/pose.go | 6 ++++++ 5 files changed, 28 insertions(+), 11 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 362f206e9d7..431291f4b9c 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -685,14 +685,18 @@ func framesAlmostEqual(frame1, frame2 Frame, epsilon float64) (bool, error) { 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 f1.geometry == nil && f2.geometry == nil: - return true, nil case !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry): return false, nil default: @@ -707,8 +711,6 @@ func framesAlmostEqual(frame1, frame2 Frame, epsilon float64) (bool, error) { switch { case !spatial.R3VectorAlmostEqual(f1.transAxis, f2.transAxis, epsilon): return false, nil - case f1.geometry == nil && f2.geometry == nil: - return true, nil case !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry): return false, nil default: diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index 7ba3c0e0c67..c7afb890cb7 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -773,7 +773,10 @@ func frameSystemsAlmostEqual(fs1, fs2 *FrameSystem, epsilon float64) (bool, erro return false, nil } - seenFrames := map[string]struct{}{} + if len(fs1.FrameNames()) != len(fs2.FrameNames()) { + return false, nil + } + for frameName, frame := range fs1.frames { frame2, ok := fs2.frames[frameName] if !ok { @@ -786,12 +789,6 @@ func frameSystemsAlmostEqual(fs1, fs2 *FrameSystem, epsilon float64) (bool, erro if !frameEquality { return false, nil } - seenFrames[frameName] = struct{}{} - } - for _, name := range fs2.FrameNames() { - if _, ok := seenFrames[name]; !ok { - return false, nil - } } return true, nil } 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) }