add gripper static obstacle test

components/camera/fake/camera.go

@@ -28,7 +28,6 @@ import (
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/rimage"
 	"go.viam.com/rdk/rimage/transform"
-	"go.viam.com/rdk/spatialmath"
 )
 
 var (
@@ -191,12 +190,6 @@ type Camera struct {
 	logger                  logging.Logger
 }
 
-// Geometries returns Geometries.
-func (c *Camera) Geometries(context.Context, map[string]interface{}) ([]spatialmath.Geometry, error) {
-	box, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{X: 40, Y: 40, Z: 10}, "box")
-	return []spatialmath.Geometry{box}, err
-}
-
 // Read always returns the same image of a yellow to blue gradient.
 func (c *Camera) Read(ctx context.Context) (image.Image, func(), error) {
 	if c.cacheImage != nil {

referenceframe/errors.go

@@ -22,6 +22,9 @@ var ErrNilPose = errors.New("pose was nil")
 // ErrMarshalingHighDOFFrame describes the error when attempting to marshal a frame with multiple degrees of freedom.
 var ErrMarshalingHighDOFFrame = errors.New("cannot marshal frame with >1 DOF, use a Model instead")
 
+// ErrNoWorldConnection describes the error when a frame system is built but nothing is connected to the world node.
+var ErrNoWorldConnection = errors.New("there are no robot parts that connect to a 'world' node. Root node must be named 'world'")
+
 // NewParentFrameMissingError returns an error for when a part has named a parent whose part is missing from the collection of Parts
 // that are becoming a FrameSystem object.
 func NewParentFrameMissingError(partName, parentName string) error {

referenceframe/frame_system.go

@@ -117,6 +117,19 @@ func NewFrameSystem(name string, parts []*FrameSystemPart, additionalTransforms
 		allParts = append(allParts, transformPart)
 	}
 
+	if len(allParts) != 0 {
+		hasWorld := false
+		for _, part := range allParts {
+			if part.FrameConfig.Parent() == World {
+				hasWorld = true
+				break
+			}
+		}
+		if !hasWorld {
+			return nil, ErrNoWorldConnection
+		}
+	}
+
 	// Topologically sort parts. After sorting, unlinked parts may reference frames
 	// that will only exist after model flattening (e.g., "arm1:joint1"). Those will
 	// be processed in a second pass after flattening.

referenceframe/transformable.go

@@ -95,7 +95,7 @@ func (pF *PoseInFrame) TransformOpt(tf *PoseInFrame) {
 
 // String returns the string representation of the PoseInFrame.
 func (pF *PoseInFrame) String() string {
-	return fmt.Sprintf("name: %v parent: %v, pose: %v", pF.name, pF.parent, pF.pose)
+	return fmt.Sprintf("parent: %s, pose: %v", pF.parent, pF.pose)
 }
 
 // MarshalJSON converts a PoseInFrame to JSON through its protobuf representation.
@@ -133,14 +133,6 @@ func NewLinkInFrame(frame string, pose spatialmath.Pose, name string, geometry s
 	}
 }
 
-// SetGeometry replaces the existing geometry with the input. This only exists to deal with the
-// clunkiness of the `LinkConfig` type that only speaks `GeometryConfig`s while we also allow for
-// resources to simply declare their `[]Geometry` objects. Which is a different kind of impedance at
-// the moment.
-func (lF *LinkInFrame) SetGeometry(geom spatialmath.Geometry) {
-	lF.geometry = geom
-}
-
 // Geometry returns the Geometry of the LinkInFrame.
 func (lF *LinkInFrame) Geometry() spatialmath.Geometry {
 	return lF.geometry

robot/framesystem/framesystem_test.go

@@ -6,6 +6,7 @@ import (
 	"testing"
 
 	"github.com/golang/geo/r3"
+	"github.com/pkg/errors"
 	"go.viam.com/test"
 
 	_ "go.viam.com/rdk/components/arm/fake"
@@ -212,6 +213,33 @@ func TestNewFrameSystemFromBadConfig(t *testing.T) {
 	ctx := context.Background()
 	logger := logging.NewTestLogger(t)
 
+	testCases := []struct {
+		name string
+		num  string
+		err  error
+	}{
+		{"no world node", "2", referenceframe.ErrNoWorldConnection},
+		{"frame named world", "3", errors.Errorf("cannot give frame system part the name %s", referenceframe.World)},
+		{"parent field empty", "4", errors.New("parent field in frame config for part \"cameraOver\" is empty")},
+	}
+
+	for _, tc := range testCases {
+		t.Run(tc.name, func(t *testing.T) {
+			cfg, err := config.Read(ctx, rdkutils.ResolveFile("robot/impl/data/fake_wrongconfig"+tc.num+".json"), logger, nil)
+			test.That(t, err, test.ShouldBeNil)
+			r, err := robotimpl.New(ctx, cfg, nil, logger)
+			test.That(t, err, test.ShouldBeNil)
+			defer r.Close(ctx)
+			fsCfg, err := r.FrameSystemConfig(ctx)
+			if err != nil {
+				test.That(t, err, test.ShouldBeError, tc.err)
+				return
+			}
+			_, err = referenceframe.NewFrameSystem(tc.num, fsCfg.Parts, nil)
+			test.That(t, err, test.ShouldBeError, tc.err)
+		})
+	}
+
 	cfg, err := config.Read(ctx, rdkutils.ResolveFile("robot/impl/data/fake.json"), logger, nil)
 	test.That(t, err, test.ShouldBeNil)
 	r, err := robotimpl.New(ctx, cfg, nil, logger)

robot/impl/data/fake_wrongconfig1.json

@@ -0,0 +1,71 @@
+{
+    "components": [
+        {
+            "name": "pieceGripper",
+            "type": "gripper",
+            "model": "fake",
+            "frame": {
+                "parent": "pieceArm"
+            }
+        },
+        {
+            "name": "cameraOver",
+            "type": "camera",
+            "model": "file",
+            "attributes": {
+                "color": "artifact_data/vision/chess/board3.png",
+                "depth": "artifact_data/vision/chess/board3.dat.gz",
+                "aligned": true
+            },
+            "frame": {
+                "parent": "world",
+                "translation": {
+                    "x": 2000,
+                    "y": 500,
+                    "z": 1300
+                },
+                "orientation": {
+                    "type": "ov_degrees",
+                    "value": {
+                        "x": 0,
+                        "y": 0,
+                        "z": 1,
+                        "th": 180
+                    }
+                }
+            }
+        },
+        {
+           "name": "pieceArm",
+            "type": "arm",
+            "model": "fake",
+            "attributes": {
+                "model-path": "../../components/arm/fake/kinematics/fake.json"
+            },
+            "frame": {
+                "parent": "world",
+                "translation": {
+                    "x": 500,
+                    "y": 500,
+                    "z": 1000
+                }
+            }
+        },
+        {
+            "name": "movement_sensor1",
+            "type": "movement_sensor",
+            "model": "fake"
+        },
+        {
+            "name": "movement_sensor2",
+            "type": "movement_sensor",
+            "model": "fake",
+            "frame": {
+                "parent": "gripperPiece"
+            },
+            "attributes": {
+                "relative": true
+            }
+        }
+    ]
+}

robot/impl/data/fake_wrongconfig2.json

@@ -0,0 +1,71 @@
+{
+    "components": [
+        {
+            "name": "pieceGripper",
+            "type": "gripper",
+            "model": "fake",
+            "frame": {
+                "parent": "pieceArm"
+            }
+        },
+        {
+            "name": "cameraOver",
+            "type": "camera",
+            "model": "file",
+            "attributes": {
+                "color": "artifact_data/vision/chess/board3.png",
+                "depth": "artifact_data/vision/chess/board3.dat.gz",
+                "aligned": true
+            },
+            "frame": {
+                "parent": "pieceArm",
+                "translation": {
+                    "x": 2000,
+                    "y": 500,
+                    "z": 1300
+                },
+                "orientation": {
+                    "type": "ov_degrees",
+                    "value": {
+                        "x": 0,
+                        "y": 0,
+                        "z": 1,
+                        "th": 180
+                    }
+                }
+            }
+        },
+        {
+           "name": "pieceArm",
+            "type": "arm",
+            "model": "fake",
+            "attributes": {
+                "model-path": "../../components/arm/fake/kinematics/fake.json"
+            },
+            "frame": {
+                "parent": "base",
+                "translation": {
+                    "x": 500,
+                    "y": 500,
+                    "z": 1000
+                }
+            }
+        },
+        {
+            "name": "movement_sensor1",
+            "type": "movement_sensor",
+            "model": "fake"
+        },
+        {
+            "name": "movement_sensor2",
+            "type": "movement_sensor",
+            "model": "fake",
+            "frame": {
+                "parent": "pieceArm"
+            },
+            "attributes": {
+                "relative": true
+            }
+        }
+    ]
+}

robot/impl/data/fake_wrongconfig3.json

@@ -0,0 +1,71 @@
+{
+    "components": [
+        {
+            "name": "pieceGripper",
+            "type": "gripper",
+            "model": "fake",
+            "frame": {
+                "parent": "pieceArm"
+            }
+        },
+        {
+            "name": "cameraOver",
+            "type": "camera",
+            "model": "file",
+            "attributes": {
+                "color": "artifact_data/vision/chess/board3.png",
+                "depth": "artifact_data/vision/chess/board3.dat.gz",
+                "aligned": true
+            },
+            "frame": {
+                "parent": "world",
+                "translation": {
+                    "x": 2000,
+                    "y": 500,
+                    "z": 1300
+                },
+                "orientation": {
+                    "type": "ov_degrees",
+                    "value": {
+                        "x": 0,
+                        "y": 0,
+                        "z": 1,
+                        "th": 180
+                    }
+                }
+            }
+        },
+        {
+           "name": "pieceArm",
+            "type": "arm",
+            "model": "fake",
+            "attributes": {
+                "model-path": "../../components/arm/fake/kinematics/fake.json"
+            },
+            "frame": {
+                "parent": "world",
+                "translation": {
+                    "x": 500,
+                    "y": 500,
+                    "z": 1000
+                }
+            }
+        },
+        {
+            "name": "movement_sensor1",
+            "type": "movement_sensor",
+            "model": "fake"
+        },
+        {
+            "model": "fake",
+            "name": "world",
+            "type": "movement_sensor",
+            "frame": {
+                "parent": "pieceGripper"
+            },
+            "attributes": {
+                "relative": true
+            }
+        }
+    ]
+}