RSDK-4290 remove MoveSingleComponent from motion service (#2751)

viamrobotics · Aug 7, 2023 · 1a7bdec · 1a7bdec
1 parent 8e094f3
commit 1a7bdec
Show file tree

Hide file tree

Showing 6 changed files with 0 additions and 180 deletions.
diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go
@@ -14,7 +14,6 @@ import (
 	geo "github.com/kellydunn/golang-geo"
 	servicepb "go.viam.com/api/service/motion/v1"
 
-	"go.viam.com/rdk/components/arm"
 	"go.viam.com/rdk/components/base"
 	"go.viam.com/rdk/components/base/fake"
 	"go.viam.com/rdk/components/base/kinematicbase"
@@ -381,62 +380,6 @@ func (ms *builtIn) planMoveOnGlobe(
 	return plan, kb, nil
 }
 
-// MoveSingleComponent will pass through a move command to a component with a MoveToPosition method that takes a pose. Arms are the only
-// component that supports this. This method will transform the destination pose, given in an arbitrary frame, into the pose of the arm.
-// The arm will then move its most distal link to that pose. If you instead wish to move any other component than the arm end to that pose,
-// then you must manually adjust the given destination by the transform from the arm end to the intended component.
-// Because this uses an arm's MoveToPosition method when issuing commands, it does not support obstacle avoidance.
-func (ms *builtIn) MoveSingleComponent(
-	ctx context.Context,
-	componentName resource.Name,
-	destination *referenceframe.PoseInFrame,
-	worldState *referenceframe.WorldState,
-	extra map[string]interface{},
-) (bool, error) {
-	operation.CancelOtherWithLabel(ctx, builtinOpLabel)
-
-	// Get the arm and all initial inputs
-	fsInputs, _, err := ms.fsService.CurrentInputs(ctx)
-	if err != nil {
-		return false, err
-	}
-	ms.logger.Debugf("frame system inputs: %v", fsInputs)
-
-	armResource, ok := ms.components[componentName]
-	if !ok {
-		return false, fmt.Errorf("could not find a resource named %v", componentName.ShortName())
-	}
-	movableArm, ok := armResource.(arm.Arm)
-	if !ok {
-		return false, fmt.Errorf(
-			"could not cast resource named %v to an arm. MoveSingleComponent only supports moving arms for now",
-			componentName,
-		)
-	}
-
-	// get destination pose in frame of movable component
-	goalPose := destination.Pose()
-	if destination.Parent() != componentName.ShortName() {
-		ms.logger.Debugf("goal given in frame of %q", destination.Parent())
-
-		frameSys, err := ms.fsService.FrameSystem(ctx, worldState.Transforms())
-		if err != nil {
-			return false, err
-		}
-
-		// re-evaluate goalPose to be in the frame we're going to move in
-		tf, err := frameSys.Transform(fsInputs, destination, componentName.ShortName()+"_origin")
-		if err != nil {
-			return false, err
-		}
-		goalPoseInFrame, _ := tf.(*referenceframe.PoseInFrame)
-		goalPose = goalPoseInFrame.Pose()
-		ms.logger.Debugf("converted goal pose %q", spatialmath.PoseToProtobuf(goalPose))
-	}
-	err = movableArm.MoveToPosition(ctx, goalPose, extra)
-	return err == nil, err
-}
-
 func (ms *builtIn) GetPose(
 	ctx context.Context,
 	componentName resource.Name,

diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go
@@ -289,51 +289,6 @@ func TestMoveWithObstacles(t *testing.T) {
 	})
 }
 
-func TestMoveSingleComponent(t *testing.T) {
-	ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json")
-	defer teardown()
-
-	grabPose := spatialmath.NewPoseFromPoint(r3.Vector{-25, 30, 0})
-	t.Run("succeeds when all frame info in config", func(t *testing.T) {
-		_, err := ms.MoveSingleComponent(
-			context.Background(),
-			arm.Named("pieceArm"),
-			referenceframe.NewPoseInFrame("c", grabPose),
-			nil,
-			map[string]interface{}{},
-		)
-		// Gripper is not an arm and cannot move
-		test.That(t, err, test.ShouldBeNil)
-	})
-	t.Run("fails due to gripper not being an arm", func(t *testing.T) {
-		_, err := ms.MoveSingleComponent(
-			context.Background(),
-			gripper.Named("pieceGripper"),
-			referenceframe.NewPoseInFrame("c", grabPose),
-			nil,
-			map[string]interface{}{},
-		)
-		// Gripper is not an arm and cannot move
-		test.That(t, err, test.ShouldNotBeNil)
-	})
-
-	t.Run("succeeds with supplemental info in world state", func(t *testing.T) {
-		worldState, err := referenceframe.NewWorldState(
-			nil,
-			[]*referenceframe.LinkInFrame{referenceframe.NewLinkInFrame("c", spatialmath.NewZeroPose(), "testFrame2", nil)},
-		)
-		test.That(t, err, test.ShouldBeNil)
-		_, err = ms.MoveSingleComponent(
-			context.Background(),
-			arm.Named("pieceArm"),
-			referenceframe.NewPoseInFrame("testFrame2", grabPose),
-			worldState,
-			map[string]interface{}{},
-		)
-		test.That(t, err, test.ShouldBeNil)
-	})
-}
-
 func TestMoveOnMapLongDistance(t *testing.T) {
 	ctx := context.Background()
 	// goal x-position of 1.32m is scaled to be in mm

diff --git a/services/motion/client.go b/services/motion/client.go
@@ -156,34 +156,6 @@ func (c *client) MoveOnGlobe(
 	return resp.Success, nil
 }
 
-func (c *client) MoveSingleComponent(
-	ctx context.Context,
-	componentName resource.Name,
-	destination *referenceframe.PoseInFrame,
-	worldState *referenceframe.WorldState,
-	extra map[string]interface{},
-) (bool, error) {
-	ext, err := vprotoutils.StructToStructPb(extra)
-	if err != nil {
-		return false, err
-	}
-	worldStateMsg, err := worldState.ToProtobuf()
-	if err != nil {
-		return false, err
-	}
-	resp, err := c.client.MoveSingleComponent(ctx, &pb.MoveSingleComponentRequest{
-		Name:          c.name,
-		ComponentName: protoutils.ResourceNameToProto(componentName),
-		Destination:   referenceframe.PoseInFrameToProtobuf(destination),
-		WorldState:    worldStateMsg,
-		Extra:         ext,
-	})
-	if err != nil {
-		return false, err
-	}
-	return resp.Success, nil
-}
-
 func (c *client) GetPose(
 	ctx context.Context,
 	componentName resource.Name,

diff --git a/services/motion/motion.go b/services/motion/motion.go
@@ -51,13 +51,6 @@ type Service interface {
 		angularVelocity float64,
 		extra map[string]interface{},
 	) (bool, error)
-	MoveSingleComponent(
-		ctx context.Context,
-		componentName resource.Name,
-		destination *referenceframe.PoseInFrame,
-		worldState *referenceframe.WorldState,
-		extra map[string]interface{},
-	) (bool, error)
 	GetPose(
 		ctx context.Context,
 		componentName resource.Name,

diff --git a/services/motion/server.go b/services/motion/server.go
@@ -108,28 +108,6 @@ func (server *serviceServer) MoveOnGlobe(ctx context.Context, req *pb.MoveOnGlob
 	return &pb.MoveOnGlobeResponse{Success: success}, err
 }
 
-func (server *serviceServer) MoveSingleComponent(
-	ctx context.Context,
-	req *pb.MoveSingleComponentRequest,
-) (*pb.MoveSingleComponentResponse, error) {
-	svc, err := server.coll.Resource(req.Name)
-	if err != nil {
-		return nil, err
-	}
-	worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState())
-	if err != nil {
-		return nil, err
-	}
-	success, err := svc.MoveSingleComponent(
-		ctx,
-		protoutils.ResourceNameFromProto(req.GetComponentName()),
-		referenceframe.ProtobufToPoseInFrame(req.GetDestination()),
-		worldState,
-		req.Extra.AsMap(),
-	)
-	return &pb.MoveSingleComponentResponse{Success: success}, err
-}
-
 func (server *serviceServer) GetPose(ctx context.Context, req *pb.GetPoseRequest) (*pb.GetPoseResponse, error) {
 	svc, err := server.coll.Resource(req.Name)
 	if err != nil {

diff --git a/testutils/inject/motion_service.go b/testutils/inject/motion_service.go
@@ -43,13 +43,6 @@ type MotionService struct {
 		angularVelocity float64,
 		extra map[string]interface{},
 	) (bool, error)
-	MoveSingleComponentFunc func(
-		ctx context.Context,
-		componentName resource.Name,
-		grabPose *referenceframe.PoseInFrame,
-		worldState *referenceframe.WorldState,
-		extra map[string]interface{},
-	) (bool, error)
 	GetPoseFunc func(
 		ctx context.Context,
 		componentName resource.Name,
@@ -119,20 +112,6 @@ func (mgs *MotionService) MoveOnGlobe(
 	return mgs.MoveOnGlobeFunc(ctx, componentName, destination, heading, movementSensorName, obstacles, linearVel, angularVel, extra)
 }
 
-// MoveSingleComponent calls the injected MoveSingleComponent or the real variant. It uses the same function as Move.
-func (mgs *MotionService) MoveSingleComponent(
-	ctx context.Context,
-	componentName resource.Name,
-	destination *referenceframe.PoseInFrame,
-	worldState *referenceframe.WorldState,
-	extra map[string]interface{},
-) (bool, error) {
-	if mgs.MoveFunc == nil {
-		return mgs.Service.MoveSingleComponent(ctx, componentName, destination, worldState, extra)
-	}
-	return mgs.MoveSingleComponentFunc(ctx, componentName, destination, worldState, extra)
-}
-
 // GetPose calls the injected GetPose or the real variant.
 func (mgs *MotionService) GetPose(
 	ctx context.Context,