RSDK-8542 expose plans generated from builtin motion service without executing them

services/motion/builtin/builtin.go

@@ -170,25 +170,13 @@
 	return nil
 }
 
-// Move takes a goal location and will plan and execute a movement to move a component specified by its name to that destination.
-func (ms *builtIn) Move(
-	ctx context.Context,
-	componentName resource.Name,
-	destination *referenceframe.PoseInFrame,
-	worldState *referenceframe.WorldState,
-	constraints *motionplan.Constraints,
-	extra map[string]interface{},
-) (bool, error) {
+func (ms *builtIn) Move(ctx context.Context, req motion.MoveReq) (bool, error) {
 	ms.mu.RLock()
 	defer ms.mu.RUnlock()
 
 	operation.CancelOtherWithLabel(ctx, builtinOpLabel)
 
-	// get goal frame
-	goalFrameName := destination.Parent()
-	ms.logger.CDebugf(ctx, "goal given in frame of %q", goalFrameName)
-
-	frameSys, err := ms.fsService.FrameSystem(ctx, worldState.Transforms())
+	frameSys, err := ms.fsService.FrameSystem(ctx, req.WorldState.Transforms())
 	if err != nil {
 		return false, err
 	}
@@ -198,17 +186,16 @@
 	if err != nil {
 		return false, err
 	}
-
-	movingFrame := frameSys.Frame(componentName.ShortName())
-
 	ms.logger.CDebugf(ctx, "frame system inputs: %v", fsInputs)
+
+	movingFrame := frameSys.Frame(req.ComponentName.ShortName())
 	if movingFrame == nil {
-		return false, fmt.Errorf("component named %s not found in robot frame system", componentName.ShortName())
+		return false, fmt.Errorf("component named %s not found in robot frame system", req.ComponentName.ShortName())
 	}
 
 	// re-evaluate goalPose to be in the frame of World
 	solvingFrame := referenceframe.World // TODO(erh): this should really be the parent of rootName
-	tf, err := frameSys.Transform(fsInputs, destination, solvingFrame)
+	tf, err := frameSys.Transform(fsInputs, &req.Destination, solvingFrame)
 	if err != nil {
 		return false, err
 	}
@@ -221,9 +208,9 @@
 		Frame:              movingFrame,
 		StartConfiguration: fsInputs,
 		FrameSystem:        frameSys,
-		WorldState:         worldState,
-		Constraints:        constraints,
-		Options:            extra,
+		WorldState:         &req.WorldState,
+		Constraints:        &req.Constraints,
+		Options:            req.Extra,
 	})
 	if err != nil {
 		return false, err
@@ -392,3 +379,13 @@
 	defer ms.mu.RUnlock()
 	return ms.state.PlanHistory(req)
 }
+
+func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) {
+	moveReq, ok := cmd["Plan"]
+	if ok {
+		// call plan 
+	}
+	if plan, ok := cmd["Execute"]; ok {
+		// call execute
+	}
+}

services/motion/client.go

@@ -9,7 +9,6 @@ import (
 	"go.viam.com/utils/rpc"
 
 	"go.viam.com/rdk/logging"
-	"go.viam.com/rdk/motionplan"
 	"go.viam.com/rdk/protoutils"
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/resource"
@@ -43,30 +42,12 @@ func NewClientFromConn(
 	return c, nil
 }
 
-func (c *client) Move(
-	ctx context.Context,
-	componentName resource.Name,
-	destination *referenceframe.PoseInFrame,
-	worldState *referenceframe.WorldState,
-	constraints *motionplan.Constraints,
-	extra map[string]interface{},
-) (bool, error) {
-	ext, err := vprotoutils.StructToStructPb(extra)
-	if err != nil {
-		return false, err
-	}
-	worldStateMsg, err := worldState.ToProtobuf()
+func (c *client) Move(ctx context.Context, req MoveReq) (bool, error) {
+	protoReq, err := req.toProto(c.name)
 	if err != nil {
 		return false, err
 	}
-	resp, err := c.client.Move(ctx, &pb.MoveRequest{
-		Name:          c.name,
-		ComponentName: protoutils.ResourceNameToProto(componentName),
-		Destination:   referenceframe.PoseInFrameToProtobuf(destination),
-		WorldState:    worldStateMsg,
-		Constraints:   constraints.ToProtobuf(),
-		Extra:         ext,
-	})
+	resp, err := c.client.Move(ctx, protoReq)
 	if err != nil {
 		return false, err
 	}

services/motion/motion.go

@@ -40,6 +40,19 @@ type PlanHistoryReq struct {
 	Extra       map[string]interface{}
 }
 
+// MoveReq describes the request to the Move interface method.
+type MoveReq struct {
+	// ComponentName of the component to move
+	ComponentName resource.Name
+	// Goal destination the component should be moved to
+	Destination referenceframe.PoseInFrame
+	// The external environment to be considered for the duration of the move
+	WorldState referenceframe.WorldState
+	// Constraints which need to be satisfied during the movement
+	Constraints motionplan.Constraints
+	Extra       map[string]interface{}
+}
+
 // MoveOnGlobeReq describes the request to the MoveOnGlobe interface method.
 type MoveOnGlobeReq struct {
 	// ComponentName of the component to move
@@ -329,14 +342,11 @@ type Service interface {
 
 	// Move is the primary method to move multiple components or any object to a specified location.
 	// Given a destination pose and a component, Move constructs a kinematic chain from goal to destination,
-	// solves it while adhering to constraints, and executes the movement to avoid collisions with the machine itself and other known objects.
+	// solves it while adhering to constraints, and executes the movement to avoid collisions with the machine itself
+	// and other known objects.
 	Move(
 		ctx context.Context,
-		componentName resource.Name,
-		destination *referenceframe.PoseInFrame,
-		worldState *referenceframe.WorldState,
-		constraints *motionplan.Constraints,
-		extra map[string]interface{},
+		req MoveReq,
 	) (bool, error)
 
 	// MoveOnMap moves a base component to a destination Pose on a SLAM map and returns a unique ExecutionID.

services/motion/pbhelpers.go

@@ -11,7 +11,9 @@ import (
 	vprotoutils "go.viam.com/utils/protoutils"
 
 	"go.viam.com/rdk/motionplan"
+	"go.viam.com/rdk/protoutils"
 	rprotoutils "go.viam.com/rdk/protoutils"
+	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/spatialmath"
 )
 
@@ -152,6 +154,40 @@ func planStateFromProto(ps pb.PlanState) PlanState {
 	}
 }
 
+// toProto converts a MoveRequest to a *pb.MoveRequest.
+func (r MoveReq) toProto(name string) (*pb.MoveRequest, error) {
+	ext, err := vprotoutils.StructToStructPb(r.Extra)
+	if err != nil {
+		return nil, err
+	}
+	worldStateMsg, err := r.WorldState.ToProtobuf()
+	if err != nil {
+		return nil, err
+	}
+	return &pb.MoveRequest{
+		Name:          name,
+		ComponentName: protoutils.ResourceNameToProto(r.ComponentName),
+		Destination:   referenceframe.PoseInFrameToProtobuf(&r.Destination),
+		WorldState:    worldStateMsg,
+		Constraints:   r.Constraints.ToProtobuf(),
+		Extra:         ext,
+	}, nil
+}
+
+func moveReqFromProto(req *pb.MoveRequest) (MoveReq, error) {
+	worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState())
+	if err != nil {
+		return MoveReq{}, err
+	}
+	return MoveReq{
+		protoutils.ResourceNameFromProto(req.GetComponentName()),
+		*referenceframe.ProtobufToPoseInFrame(req.GetDestination()),
+		*worldState,
+		*motionplan.ConstraintsFromProtobuf(req.GetConstraints()),
+		req.Extra.AsMap(),
+	}, nil
+}
+
 // toProto converts a MoveOnGlobeRequest to a *pb.MoveOnGlobeRequest.
 func (r MoveOnGlobeReq) toProto(name string) (*pb.MoveOnGlobeRequest, error) {
 	ext, err := vprotoutils.StructToStructPb(r.Extra)

services/motion/server.go

@@ -7,7 +7,6 @@ import (
 	commonpb "go.viam.com/api/common/v1"
 	pb "go.viam.com/api/service/motion/v1"
 
-	"go.viam.com/rdk/motionplan"
 	"go.viam.com/rdk/protoutils"
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/resource"
@@ -30,18 +29,11 @@ func (server *serviceServer) Move(ctx context.Context, req *pb.MoveRequest) (*pb
 	if err != nil {
 		return nil, err
 	}
-	worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState())
+	r, err := moveReqFromProto(req)
 	if err != nil {
 		return nil, err
 	}
-	success, err := svc.Move(
-		ctx,
-		protoutils.ResourceNameFromProto(req.GetComponentName()),
-		referenceframe.ProtobufToPoseInFrame(req.GetDestination()),
-		worldState,
-		motionplan.ConstraintsFromProtobuf(req.GetConstraints()),
-		req.Extra.AsMap(),
-	)
+	success, err := svc.Move(ctx, r)
 	return &pb.MoveResponse{Success: success}, err
 }
 

testutils/inject/motion_service.go

@@ -3,7 +3,6 @@ package inject
 import (
 	"context"
 
-	"go.viam.com/rdk/motionplan"
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/services/motion"
@@ -16,11 +15,7 @@ type MotionService struct {
 	name     resource.Name
 	MoveFunc func(
 		ctx context.Context,
-		componentName resource.Name,
-		grabPose *referenceframe.PoseInFrame,
-		worldState *referenceframe.WorldState,
-		constraints *motionplan.Constraints,
-		extra map[string]interface{},
+		req motion.MoveReq,
 	) (bool, error)
 	MoveOnMapFunc func(
 		ctx context.Context,
@@ -65,18 +60,11 @@ func (mgs *MotionService) Name() resource.Name {
 }
 
 // Move calls the injected Move or the real variant.
-func (mgs *MotionService) Move(
-	ctx context.Context,
-	componentName resource.Name,
-	destination *referenceframe.PoseInFrame,
-	worldState *referenceframe.WorldState,
-	constraints *motionplan.Constraints,
-	extra map[string]interface{},
-) (bool, error) {
+func (mgs *MotionService) Move(ctx context.Context, req motion.MoveReq) (bool, error) {
 	if mgs.MoveFunc == nil {
-		return mgs.Service.Move(ctx, componentName, destination, worldState, constraints, extra)
+		return mgs.Service.Move(ctx, req)
 	}
-	return mgs.MoveFunc(ctx, componentName, destination, worldState, constraints, extra)
+	return mgs.MoveFunc(ctx, req)
 }
 
 // MoveOnMap calls the injected MoveOnMap or the real variant.