RSDK-8542 expose plans generated from builtin motion service without …

…executing them (viamrobotics#4287)

components/arm/arm.go

@@ -17,6 +17,7 @@ import (
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/robot"
+	"go.viam.com/rdk/robot/framesystem"
 	"go.viam.com/rdk/spatialmath"
 )
 
@@ -98,7 +99,7 @@ type Arm interface {
 	referenceframe.ModelFramer
 	resource.Shaped
 	resource.Actuator
-	referenceframe.InputEnabled
+	framesystem.InputEnabled
 
 	// EndPosition returns the current position of the arm.
 	EndPosition(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error)

components/base/kinematicbase/kinematics.go

@@ -13,14 +13,15 @@ import (
 	"go.viam.com/rdk/logging"
 	"go.viam.com/rdk/motionplan"
 	"go.viam.com/rdk/referenceframe"
+	"go.viam.com/rdk/robot/framesystem"
 	"go.viam.com/rdk/services/motion"
 )
 
 // KinematicBase is an interface for Bases that also satisfy the ModelFramer and InputEnabled interfaces.
 type KinematicBase interface {
 	base.Base
 	motion.Localizer
-	referenceframe.InputEnabled
+	framesystem.InputEnabled
 
 	Kinematics() referenceframe.Frame
 	LocalizationFrame() referenceframe.Frame

components/gantry/gantry.go

@@ -13,6 +13,7 @@ import (
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/robot"
+	"go.viam.com/rdk/robot/framesystem"
 )
 
 func init() {
@@ -85,7 +86,7 @@ type Gantry interface {
 	resource.Resource
 	resource.Actuator
 	referenceframe.ModelFramer
-	referenceframe.InputEnabled
+	framesystem.InputEnabled
 
 	// Position returns the position in meters.
 	Position(ctx context.Context, extra map[string]interface{}) ([]float64, error)

motionplan/constraint.go

@@ -600,7 +600,7 @@ func NewConstraints(
 // ConstraintsFromProtobuf converts a protobuf object to a Constraints object.
 func ConstraintsFromProtobuf(pbConstraint *motionpb.Constraints) *Constraints {
 	if pbConstraint == nil {
-		return nil
+		return NewEmptyConstraints()
 	}
 
 	// iterate through all motionpb.LinearConstraint and convert to RDK form

referenceframe/primitives.go

@@ -1,7 +1,6 @@
 package referenceframe
 
 import (
-	"context"
 	"fmt"
 	"math"
 
@@ -56,14 +55,6 @@ func JointPositionsFromRadians(radians []float64) *pb.JointPositions {
 	return &pb.JointPositions{Values: n}
 }
 
-// InputEnabled is a standard interface for all things that interact with the frame system
-// This allows us to figure out where they currently are, and then move them.
-// Input units are always in meters or radians.
-type InputEnabled interface {
-	CurrentInputs(ctx context.Context) ([]Input, error)
-	GoToInputs(context.Context, ...[]Input) error
-}
-
 // interpolateInputs will return a set of inputs that are the specified percent between the two given sets of
 // inputs. For example, setting by to 0.5 will return the inputs halfway between the from/to values, and 0.25 would
 // return one quarter of the way from "from" to "to".

robot/framesystem/framesystem.go

@@ -30,6 +30,14 @@ var API = resource.APINamespaceRDKInternal.WithServiceType(SubtypeName)
 // InternalServiceName is used to refer to/depend on this service internally.
 var InternalServiceName = resource.NewName(API, "builtin")
 
+// InputEnabled is a standard interface for all things that interact with the frame system
+// This allows us to figure out where they currently are, and then move them.
+// Input units are always in meters or radians.
+type InputEnabled interface {
+	CurrentInputs(ctx context.Context) ([]referenceframe.Input, error)
+	GoToInputs(context.Context, ...[]referenceframe.Input) error
+}
+
 // A Service that returns the frame system for a robot.
 //
 // TransformPose example:
@@ -73,7 +81,7 @@ type Service interface {
 
 	// CurrentInputs returns a map of the current inputs for each component of a machine's frame system
 	// and a map of statuses indicating which of the machine's components may be actuated through input values.
-	CurrentInputs(ctx context.Context) (map[string][]referenceframe.Input, map[string]referenceframe.InputEnabled, error)
+	CurrentInputs(ctx context.Context) (map[string][]referenceframe.Input, map[string]InputEnabled, error)
 
 	// FrameSystem returns the frame system of the machine and incorporates any specified additional transformations.
 	FrameSystem(ctx context.Context, additionalTransforms []*referenceframe.LinkInFrame) (referenceframe.FrameSystem, error)
@@ -222,7 +230,7 @@ func (svc *frameSystemService) TransformPose(
 		if !ok {
 			return nil, DependencyNotFoundError(name)
 		}
-		inputEnabled, ok := component.(referenceframe.InputEnabled)
+		inputEnabled, ok := component.(InputEnabled)
 		if !ok {
 			return nil, NotInputEnabledError(component)
 		}
@@ -247,15 +255,15 @@ func (svc *frameSystemService) TransformPose(
 // InputEnabled resources that those inputs came from.
 func (svc *frameSystemService) CurrentInputs(
 	ctx context.Context,
-) (map[string][]referenceframe.Input, map[string]referenceframe.InputEnabled, error) {
+) (map[string][]referenceframe.Input, map[string]InputEnabled, error) {
 	fs, err := svc.FrameSystem(ctx, []*referenceframe.LinkInFrame{})
 	if err != nil {
 		return nil, nil, err
 	}
 	input := referenceframe.StartPositions(fs)
 
 	// build maps of relevant components and inputs from initial inputs
-	resources := map[string]referenceframe.InputEnabled{}
+	resources := map[string]InputEnabled{}
 	for name, original := range input {
 		// skip frames with no input
 		if len(original) == 0 {
@@ -267,7 +275,7 @@ func (svc *frameSystemService) CurrentInputs(
 		if !ok {
 			return nil, nil, DependencyNotFoundError(name)
 		}
-		inputEnabled, ok := component.(referenceframe.InputEnabled)
+		inputEnabled, ok := component.(InputEnabled)
 		if !ok {
 			return nil, nil, NotInputEnabledError(component)
 		}

robot/impl/resource_manager_test.go

@@ -51,7 +51,6 @@ import (
 	"go.viam.com/rdk/grpc"
 	"go.viam.com/rdk/logging"
 	"go.viam.com/rdk/module/modmaninterface"
-	"go.viam.com/rdk/motionplan"
 	"go.viam.com/rdk/operation"
 	"go.viam.com/rdk/pointcloud"
 	"go.viam.com/rdk/referenceframe"
@@ -465,16 +464,6 @@ func TestManagerAdd(t *testing.T) {
 	test.That(t, resource1, test.ShouldEqual, injectBoard)
 
 	injectMotionService := &inject.MotionService{}
-	injectMotionService.MoveFunc = func(
-		ctx context.Context,
-		componentName resource.Name,
-		grabPose *referenceframe.PoseInFrame,
-		worldState *referenceframe.WorldState,
-		constraints *motionplan.Constraints,
-		extra map[string]interface{},
-	) (bool, error) {
-		return false, nil
-	}
 	objectMResName := motion.Named("motion1")
 	manager.resources.AddNode(objectMResName, resource.NewConfiguredGraphNode(resource.Config{}, injectMotionService, unknownModel))
 	motionService, err := manager.ResourceByName(objectMResName)