RSDK-4120 add MoveThroughJointPositions to Arm interface (#4508)

components/arm/arm.go

@@ -75,17 +75,26 @@ func Named(name string) resource.Name {
 //
 // MoveToJointPositions example:
 //
-//	// Assumes you have imported "go.viam.com/api/component/arm/v1" as `componentpb`
 //	myArm, err := arm.FromRobot(machine, "my_arm")
 //
-//	// Declare an array of values with your desired rotational value for each joint on the arm.
-//	degrees := []float64{4.0, 5.0, 6.0}
+//	// Declare an array of values with your desired rotational value (in radians) for each joint on the arm.
+//	inputs := referenceframe.FloatsToInputs([]float64{0, math.Pi/2, math.Pi})
 //
-//	// Declare a new JointPositions with these values.
-//	jointPos := &componentpb.JointPositions{Values: degrees}
+//	// Move each joint of the arm to the positions specified in the above slice
+//	err = myArm.MoveToJointPositions(context.Background(), inputs, nil)
 //
-//	// Move each joint of the arm to the position these values specify.
-//	err = myArm.MoveToJointPositions(context.Background(), jointPos, nil)
+// MoveToJointPositions example:
+//
+//	myArm, err := arm.FromRobot(machine, "my_arm")
+//
+//	// Declare a 2D array of values with your desired rotational value (in radians) for each joint on the arm.
+//	inputs := [][]referenceframe.Input{
+//		referenceframe.FloatsToInputs([]float64{0, math.Pi/2, math.Pi})
+//		referenceframe.FloatsToInputs([]float64{0, 0, 0})
+//	}
+//
+//	// Move each joint of the arm through the positions in the slice defined above
+//	err = myArm.MoveThroughJointPositions(context.Background(), inputs, nil, nil)
 //
 // JointPositions example:
 //
@@ -113,6 +122,10 @@ type Arm interface {
 	// This will block until done or a new operation cancels this one.
 	MoveToJointPositions(ctx context.Context, positions []referenceframe.Input, extra map[string]interface{}) error
 
+	// MoveThroughJointPositions moves the arm's joints through the given positions in the order they are specified.
+	// This will block until done or a new operation cancels this one.
+	MoveThroughJointPositions(ctx context.Context, positions [][]referenceframe.Input, options *MoveOptions, extra map[string]any) error
+
 	// JointPositions returns the current joint positions of the arm.
 	JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error)
 }
@@ -159,22 +172,6 @@ func CreateStatus(ctx context.Context, a Arm) (*pb.Status, error) {
 	return &pb.Status{EndPosition: endPosition, JointPositions: jointPositions, IsMoving: isMoving}, nil
 }
 
-// GoToWaypoints will visit in turn each of the joint position waypoints generated by a motion planner.
-func GoToWaypoints(ctx context.Context, a Arm, waypoints [][]referenceframe.Input) error {
-	for _, waypoint := range waypoints {
-		err := ctx.Err() // make sure we haven't been cancelled
-		if err != nil {
-			return err
-		}
-
-		err = a.GoToInputs(ctx, waypoint)
-		if err != nil {
-			return err
-		}
-	}
-	return nil
-}
-
 // CheckDesiredJointPositions validates that the desired joint positions either bring the joint back
 // in bounds or do not move the joint more out of bounds.
 func CheckDesiredJointPositions(ctx context.Context, a Arm, desiredInputs []referenceframe.Input) error {

components/arm/client.go

@@ -94,9 +94,6 @@ func (c *client) MoveToJointPositions(ctx context.Context, positions []reference
 	if err != nil {
 		return err
 	}
-	if positions == nil {
-		c.logger.Warnf("%s MoveToJointPositions: position parameter is nil", c.name)
-	}
 	jp, err := referenceframe.JointPositionsFromInputs(c.model, positions)
 	if err != nil {
 		return err
@@ -109,6 +106,39 @@ func (c *client) MoveToJointPositions(ctx context.Context, positions []reference
 	return err
 }
 
+func (c *client) MoveThroughJointPositions(
+	ctx context.Context,
+	positions [][]referenceframe.Input,
+	options *MoveOptions,
+	extra map[string]interface{},
+) error {
+	ext, err := protoutils.StructToStructPb(extra)
+	if err != nil {
+		return err
+	}
+	if positions == nil {
+		c.logger.Warnf("%s MoveThroughJointPositions: position argument is nil", c.name)
+	}
+	allJPs := make([]*pb.JointPositions, 0, len(positions))
+	for _, position := range positions {
+		jp, err := referenceframe.JointPositionsFromInputs(c.model, position)
+		if err != nil {
+			return err
+		}
+		allJPs = append(allJPs, jp)
+	}
+	req := &pb.MoveThroughJointPositionsRequest{
+		Name:      c.name,
+		Positions: allJPs,
+		Extra:     ext,
+	}
+	if options != nil {
+		req.Options = options.toProtobuf()
+	}
+	_, err = c.client.MoveThroughJointPositions(ctx, req)
+	return err
+}
+
 func (c *client) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) {
 	ext, err := protoutils.StructToStructPb(extra)
 	if err != nil {
@@ -145,13 +175,7 @@ func (c *client) CurrentInputs(ctx context.Context) ([]referenceframe.Input, err
 }
 
 func (c *client) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error {
-	// TODO: switch this over call MoveThroughJointPositions when that API is ready
-	for _, goal := range inputSteps {
-		if err := c.MoveToJointPositions(ctx, goal, nil); err != nil {
-			return err
-		}
-	}
-	return nil
+	return c.MoveThroughJointPositions(ctx, inputSteps, nil, nil)
 }
 
 func (c *client) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) {

components/arm/client_test.go

@@ -32,12 +32,14 @@ func TestClient(t *testing.T) {
 	var (
 		capArmPos      spatialmath.Pose
 		capArmJointPos []referenceframe.Input
+		moveOptions    arm.MoveOptions
 		extraOptions   map[string]interface{}
 	)
 
 	pos1 := spatialmath.NewPoseFromPoint(r3.Vector{X: 1, Y: 2, Z: 3})
 	jointPos1 := []referenceframe.Input{{1.}, {2.}, {3.}}
 	expectedGeometries := []spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{1, 2, 3}, "")}
+	expectedMoveOptions := arm.MoveOptions{MaxVelRads: 1, MaxAccRads: 2}
 	injectArm := &inject.Arm{}
 	injectArm.EndPositionFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) {
 		extraOptions = extra
@@ -52,12 +54,22 @@ func TestClient(t *testing.T) {
 		extraOptions = extra
 		return nil
 	}
-
 	injectArm.MoveToJointPositionsFunc = func(ctx context.Context, jp []referenceframe.Input, extra map[string]interface{}) error {
 		capArmJointPos = jp
 		extraOptions = extra
 		return nil
 	}
+	injectArm.MoveThroughJointPositionsFunc = func(
+		ctx context.Context,
+		positions [][]referenceframe.Input,
+		options *arm.MoveOptions,
+		extra map[string]interface{},
+	) error {
+		capArmJointPos = positions[len(positions)-1]
+		moveOptions = *options
+		extraOptions = extra
+		return nil
+	}
 	injectArm.StopFunc = func(ctx context.Context, extra map[string]interface{}) error {
 		extraOptions = extra
 		return errStopUnimplemented
@@ -82,7 +94,6 @@ func TestClient(t *testing.T) {
 		capArmPos = ap
 		return nil
 	}
-
 	injectArm2.MoveToJointPositionsFunc = func(ctx context.Context, jp []referenceframe.Input, extra map[string]interface{}) error {
 		capArmJointPos = jp
 		return nil
@@ -94,11 +105,10 @@ func TestClient(t *testing.T) {
 		return nil
 	}
 
-	armSvc, err := resource.NewAPIResourceCollection(
-		arm.API, map[resource.Name]arm.Arm{
-			arm.Named(testArmName):  injectArm,
-			arm.Named(testArmName2): injectArm2,
-		})
+	armSvc, err := resource.NewAPIResourceCollection(arm.API, map[resource.Name]arm.Arm{
+		arm.Named(testArmName):  injectArm,
+		arm.Named(testArmName2): injectArm2,
+	})
 	test.That(t, err, test.ShouldBeNil)
 	resourceAPI, ok, err := resource.LookupAPIRegistration[arm.Arm](arm.API)
 	test.That(t, err, test.ShouldBeNil)
@@ -164,6 +174,17 @@ func TestClient(t *testing.T) {
 		test.That(t, capArmJointPos, test.ShouldResemble, jointPos2)
 		test.That(t, extraOptions, test.ShouldResemble, map[string]interface{}{"foo": "MoveToJointPositions"})
 
+		err = arm1Client.MoveThroughJointPositions(
+			context.Background(),
+			[][]referenceframe.Input{jointPos2, jointPos1},
+			&expectedMoveOptions,
+			map[string]interface{}{"foo": "MoveThroughJointPositions"},
+		)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, capArmJointPos, test.ShouldResemble, jointPos1)
+		test.That(t, moveOptions, test.ShouldResemble, expectedMoveOptions)
+		test.That(t, extraOptions, test.ShouldResemble, map[string]interface{}{"foo": "MoveThroughJointPositions"})
+
 		err = arm1Client.Stop(context.Background(), map[string]interface{}{"foo": "Stop"})
 		test.That(t, err, test.ShouldNotBeNil)
 		test.That(t, err.Error(), test.ShouldContainSubstring, errStopUnimplemented.Error())

components/arm/eva/eva.go

@@ -177,6 +177,24 @@ func (e *eva) MoveToJointPositions(ctx context.Context, newPositions []reference
 	return e.doMoveJoints(ctx, radians)
 }
 
+func (e *eva) MoveThroughJointPositions(
+	ctx context.Context,
+	positions [][]referenceframe.Input,
+	_ *arm.MoveOptions,
+	_ map[string]interface{},
+) error {
+	for _, goal := range positions {
+		if err := arm.CheckDesiredJointPositions(ctx, e, goal); err != nil {
+			return err
+		}
+		err := e.MoveToJointPositions(ctx, goal, nil)
+		if err != nil {
+			return err
+		}
+	}
+	return nil
+}
+
 func (e *eva) doMoveJoints(ctx context.Context, joints []float64) error {
 	if err := e.apiLock(ctx); err != nil {
 		return err
@@ -384,16 +402,7 @@ func (e *eva) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error)
 }
 
 func (e *eva) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error {
-	for _, goal := range inputSteps {
-		if err := arm.CheckDesiredJointPositions(ctx, e, goal); err != nil {
-			return err
-		}
-		err := e.MoveToJointPositions(ctx, goal, nil)
-		if err != nil {
-			return err
-		}
-	}
-	return nil
+	return e.MoveThroughJointPositions(ctx, inputSteps, nil, nil)
 }
 
 func (e *eva) Close(ctx context.Context) error {

components/arm/fake/fake.go

@@ -188,6 +188,21 @@ func (a *Arm) MoveToJointPositions(ctx context.Context, joints []referenceframe.
 	return nil
 }
 
+// MoveThroughJointPositions moves the fake arm through the given inputs.
+func (a *Arm) MoveThroughJointPositions(
+	ctx context.Context,
+	positions [][]referenceframe.Input,
+	_ *arm.MoveOptions,
+	_ map[string]interface{},
+) error {
+	for _, goal := range positions {
+		if err := a.MoveToJointPositions(ctx, goal, nil); err != nil {
+			return err
+		}
+	}
+	return nil
+}
+
 // JointPositions returns joints.
 func (a *Arm) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) {
 	a.mu.RLock()
@@ -214,12 +229,7 @@ func (a *Arm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error)
 
 // GoToInputs moves the fake arm to the given inputs.
 func (a *Arm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error {
-	for _, goal := range inputSteps {
-		if err := a.MoveToJointPositions(ctx, goal, nil); err != nil {
-			return err
-		}
-	}
-	return nil
+	return a.MoveThroughJointPositions(ctx, inputSteps, nil, nil)
 }
 
 // Close does nothing.

components/arm/pb_helpers.go

@@ -4,11 +4,45 @@ import (
 	"fmt"
 
 	commonpb "go.viam.com/api/common/v1"
+	pb "go.viam.com/api/component/arm/v1"
 
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/referenceframe/urdf"
+	"go.viam.com/rdk/utils"
 )
 
+// MoveOptions define parameters to be obeyed during arm movement.
+type MoveOptions struct {
+	MaxVelRads, MaxAccRads float64
+}
+
+func moveOptionsFromProtobuf(protobuf *pb.MoveOptions) *MoveOptions {
+	if protobuf == nil {
+		protobuf = &pb.MoveOptions{}
+	}
+
+	var vel, acc float64
+	if protobuf.MaxVelDegsPerSec != nil {
+		vel = *protobuf.MaxVelDegsPerSec
+	}
+	if protobuf.MaxAccDegsPerSec2 != nil {
+		acc = *protobuf.MaxAccDegsPerSec2
+	}
+	return &MoveOptions{
+		MaxVelRads: utils.DegToRad(vel),
+		MaxAccRads: utils.DegToRad(acc),
+	}
+}
+
+func (opts *MoveOptions) toProtobuf() *pb.MoveOptions {
+	vel := utils.RadToDeg(opts.MaxVelRads)
+	acc := utils.RadToDeg(opts.MaxAccRads)
+	return &pb.MoveOptions{
+		MaxVelDegsPerSec:  &vel,
+		MaxAccDegsPerSec2: &acc,
+	}
+}
+
 func parseKinematicsResponse(name string, resp *commonpb.GetKinematicsResponse) (referenceframe.Model, error) {
 	format := resp.GetFormat()
 	data := resp.GetKinematicsData()

components/arm/server.go

@@ -103,6 +103,28 @@ func (s *serviceServer) MoveToJointPositions(
 	return &pb.MoveToJointPositionsResponse{}, arm.MoveToJointPositions(ctx, inputs, req.Extra.AsMap())
 }
 
+// MoveThroughJointPositions moves an arm of the underlying robot through the requested joint positions.
+func (s *serviceServer) MoveThroughJointPositions(
+	ctx context.Context,
+	req *pb.MoveThroughJointPositionsRequest,
+) (*pb.MoveThroughJointPositionsResponse, error) {
+	operation.CancelOtherWithLabel(ctx, req.Name)
+	arm, err := s.coll.Resource(req.Name)
+	if err != nil {
+		return nil, err
+	}
+	allInputs := make([][]referenceframe.Input, 0, len(req.Positions))
+	for _, position := range req.Positions {
+		inputs, err := referenceframe.InputsFromJointPositions(arm.ModelFrame(), position)
+		if err != nil {
+			return nil, err
+		}
+		allInputs = append(allInputs, inputs)
+	}
+	err = arm.MoveThroughJointPositions(ctx, allInputs, moveOptionsFromProtobuf(req.Options), req.Extra.AsMap())
+	return &pb.MoveThroughJointPositionsResponse{}, err
+}
+
 // Stop stops the arm specified.
 func (s *serviceServer) Stop(ctx context.Context, req *pb.StopRequest) (*pb.StopResponse, error) {
 	operation.CancelOtherWithLabel(ctx, req.Name)