From b05a74ff1d6c41248ae516b395116005af1a6563 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 24 Oct 2024 15:44:10 -0400 Subject: [PATCH 01/14] update go mod --- go.mod | 2 +- go.sum | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/go.mod b/go.mod index fac647b9ef8..6474e068033 100644 --- a/go.mod +++ b/go.mod @@ -85,7 +85,7 @@ require ( go.uber.org/atomic v1.11.0 go.uber.org/multierr v1.11.0 go.uber.org/zap v1.27.0 - go.viam.com/api v0.1.350 + go.viam.com/api v0.1.351 go.viam.com/test v1.1.1-0.20220913152726-5da9916c08a2 go.viam.com/utils v0.1.106 goji.io v2.0.2+incompatible diff --git a/go.sum b/go.sum index b5c7e8b740a..e244c7d0e89 100644 --- a/go.sum +++ b/go.sum @@ -1647,6 +1647,8 @@ go.uber.org/zap v1.27.0 h1:aJMhYGrd5QSmlpLMr2MftRKl7t8J8PTZPA732ud/XR8= go.uber.org/zap v1.27.0/go.mod h1:GB2qFLM7cTU87MWRP2mPIjqfIDnGu+VIO4V/SdhGo2E= go.viam.com/api v0.1.350 h1:pMyU/fFS4cEsQNQoC69lBapwl6R2jbN2q4hoT+Sz8lw= go.viam.com/api v0.1.350/go.mod h1:5lpVRxMsKFCaahqsnJfPGwJ9baoQ6PIKQu3lxvy6Wtw= +go.viam.com/api v0.1.351 h1:jNIpa7K1RzQzWrwnXJBwFZ+oQ/OiZBWLqTp9+7nznPI= +go.viam.com/api v0.1.351/go.mod h1:5lpVRxMsKFCaahqsnJfPGwJ9baoQ6PIKQu3lxvy6Wtw= go.viam.com/test v1.1.1-0.20220913152726-5da9916c08a2 h1:oBiK580EnEIzgFLU4lHOXmGAE3MxnVbeR7s1wp/F3Ps= go.viam.com/test v1.1.1-0.20220913152726-5da9916c08a2/go.mod h1:XM0tej6riszsiNLT16uoyq1YjuYPWlRBweTPRDanIts= go.viam.com/utils v0.1.106 h1:LcmVUBBV7ipzl4to4RJ4knj5bVdATU8reO77HNMimdA= From 418dd2d9e91944a72fdd466332b73de817703b3b Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Tue, 29 Oct 2024 14:14:29 -0400 Subject: [PATCH 02/14] passing tests in arm package --- components/arm/arm.go | 25 +++++++------ components/arm/arm_test.go | 28 ++------------ components/arm/client.go | 37 ++++++++----------- components/arm/client_test.go | 29 ++++++--------- components/arm/collector.go | 9 +++-- components/arm/collectors_test.go | 18 ++++----- components/arm/eva/eva.go | 24 +++++------- components/arm/fake/fake.go | 20 ++++------ components/arm/fake/fake_test.go | 10 ++--- components/arm/server.go | 22 +++++------ components/arm/server_test.go | 32 ++++++++-------- components/arm/universalrobots/ur.go | 21 ++++------- components/arm/wrapper/wrapper.go | 17 +++------ components/arm/xarm/xarm.go | 9 +---- components/arm/xarm/xarm_comm.go | 12 +++--- motionplan/motionPlanner.go | 4 +- motionplan/solverFrame.go | 8 ++-- motionplan/tpspace/ptgDiffDrive.go | 2 +- motionplan/tpspace/ptgGroupFrame.go | 2 +- motionplan/tpspace/ptgIKFrame.go | 2 +- referenceframe/errors.go | 6 +-- referenceframe/frame.go | 14 +++---- referenceframe/{primitives.go => input.go} | 28 ++++++++++++-- .../{primitives_test.go => input_test.go} | 0 referenceframe/model.go | 2 +- referenceframe/model_test.go | 4 +- robot/client/client_test.go | 4 +- robot/impl/local_robot_test.go | 8 ++-- services/datamanager/builtin/builtin_test.go | 6 +-- services/motion/builtin/wrapperFrame.go | 8 ++-- services/motion/motion_helpers_test.go | 9 ++--- testutils/inject/arm.go | 14 +++---- 32 files changed, 196 insertions(+), 238 deletions(-) rename referenceframe/{primitives.go => input.go} (73%) rename referenceframe/{primitives_test.go => input_test.go} (100%) diff --git a/components/arm/arm.go b/components/arm/arm.go index 8615fb193d4..2f86aca4871 100644 --- a/components/arm/arm.go +++ b/components/arm/arm.go @@ -8,6 +8,7 @@ package arm import ( "context" + "errors" "fmt" v1 "go.viam.com/api/common/v1" @@ -22,6 +23,8 @@ import ( "go.viam.com/rdk/utils" ) +var ErrModelNotValid = errors.New("unable to retrieve a valid arm model from arm") + func init() { resource.RegisterAPI(API, resource.APIRegistration[Arm]{ Status: resource.StatusFunc(CreateStatus), @@ -111,10 +114,10 @@ type Arm interface { // MoveToJointPositions moves the arm's joints to the given positions. // This will block until done or a new operation cancels this one. - MoveToJointPositions(ctx context.Context, positionDegs *pb.JointPositions, extra map[string]interface{}) error + MoveToJointPositions(ctx context.Context, positions []referenceframe.Input, extra map[string]interface{}) error // JointPositions returns the current joint positions of the arm. - JointPositions(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) + JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) } // FromDependencies is a helper for getting the named arm from a collection of @@ -136,19 +139,20 @@ func NamesFromRobot(r robot.Robot) []string { // CreateStatus creates a status from the arm. This will report calculated end effector positions even if the given // arm is perceived to be out of bounds. func CreateStatus(ctx context.Context, a Arm) (*pb.Status, error) { - var jointPositions *pb.JointPositions + model := a.ModelFrame() joints, err := a.JointPositions(ctx, nil) if err != nil { return nil, err } - jointPositions = joints var endPosition *v1.Pose - model := a.ModelFrame() - if model != nil && joints != nil { - if endPose, err := referenceframe.ComputeOOBPosition(model, model.InputFromProtobuf(jointPositions)); err == nil { - endPosition = spatialmath.PoseToProtobuf(endPose) - } + if endPose, err := referenceframe.ComputeOOBPosition(model, joints); err == nil { + endPosition = spatialmath.PoseToProtobuf(endPose) + } + + var jointPositions *pb.JointPositions + if jp, err := referenceframe.JointPositionsFromInputs(model, joints); err == nil { + jointPositions = jp } isMoving, err := a.IsMoving(ctx) @@ -182,12 +186,11 @@ func CheckDesiredJointPositions(ctx context.Context, a Arm, desiredInputs []refe return err } model := a.ModelFrame() - checkPositions := model.InputFromProtobuf(currentJointPos) limits := model.DoF() for i, val := range desiredInputs { max := limits[i].Max min := limits[i].Min - currPosition := checkPositions[i] + currPosition := currentJointPos[i] // to make sure that val is a valid input it must either bring the joint closer inbounds or keep the joint inbounds. if currPosition.Value > limits[i].Max { max = currPosition.Value diff --git a/components/arm/arm_test.go b/components/arm/arm_test.go index 4036c9170ef..2750429a99a 100644 --- a/components/arm/arm_test.go +++ b/components/arm/arm_test.go @@ -73,8 +73,8 @@ func TestCreateStatus(t *testing.T) { injectArm := &inject.Arm{} //nolint:unparam - successfulJointPositionsFunc := func(context.Context, map[string]interface{}) (*pb.JointPositions, error) { - return successfulStatus.JointPositions, nil + successfulJointPositionsFunc := func(context.Context, map[string]interface{}) ([]referenceframe.Input, error) { + return referenceframe.FloatsToInputs(referenceframe.JointPositionsToRadians(successfulStatus.JointPositions)), nil } successfulIsMovingFunc := func(context.Context) (bool, error) { @@ -161,7 +161,7 @@ func TestCreateStatus(t *testing.T) { injectArm.ModelFrameFunc = successfulModelFrameFunc errFail := errors.New("can't get joint positions") - injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { + injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { return nil, errFail } @@ -170,31 +170,9 @@ func TestCreateStatus(t *testing.T) { test.That(t, actualStatus, test.ShouldBeNil) }) - t.Run("nil JointPositions", func(t *testing.T) { - injectArm.IsMovingFunc = successfulIsMovingFunc - injectArm.ModelFrameFunc = successfulModelFrameFunc - - injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { - return nil, nil //nolint:nilnil - } - - expectedStatus := &pb.Status{ - EndPosition: nil, - JointPositions: nil, - IsMoving: successfulStatus.IsMoving, - } - - actualStatus, err := arm.CreateStatus(context.Background(), injectArm) - test.That(t, err, test.ShouldBeNil) - test.That(t, actualStatus.EndPosition, test.ShouldEqual, expectedStatus.EndPosition) - test.That(t, actualStatus.JointPositions, test.ShouldEqual, expectedStatus.JointPositions) - test.That(t, actualStatus.IsMoving, test.ShouldEqual, expectedStatus.IsMoving) - }) - t.Run("nil model frame", func(t *testing.T) { injectArm.IsMovingFunc = successfulIsMovingFunc injectArm.JointPositionsFunc = successfulJointPositionsFunc - injectArm.ModelFrameFunc = func() referenceframe.Model { return nil } diff --git a/components/arm/client.go b/components/arm/client.go index d1b060d03a1..42d0ad3c597 100644 --- a/components/arm/client.go +++ b/components/arm/client.go @@ -5,7 +5,6 @@ package arm import ( "context" - "errors" commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/arm/v1" @@ -19,8 +18,6 @@ import ( "go.viam.com/rdk/spatialmath" ) -var errArmClientModelNotValid = errors.New("unable to retrieve a valid arm model from arm client") - // client implements ArmServiceClient. type client struct { resource.Named @@ -49,7 +46,12 @@ func NewClientFromConn( } clientFrame, err := c.updateKinematics(ctx, nil) if err != nil { - logger.CErrorw(ctx, "error getting model for arm; will not allow certain methods", "err", err) + logger.CWarnw( + ctx, + "error getting model for arm; making the assumption that joints are revolute and that their positions are specified in degrees", + "err", + err, + ) } else { c.model = clientFrame } @@ -87,7 +89,7 @@ func (c *client) MoveToPosition(ctx context.Context, pose spatialmath.Pose, extr return err } -func (c *client) MoveToJointPositions(ctx context.Context, positions *pb.JointPositions, extra map[string]interface{}) error { +func (c *client) MoveToJointPositions(ctx context.Context, positions []referenceframe.Input, extra map[string]interface{}) error { ext, err := protoutils.StructToStructPb(extra) if err != nil { return err @@ -95,15 +97,19 @@ func (c *client) MoveToJointPositions(ctx context.Context, positions *pb.JointPo 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 + } _, err = c.client.MoveToJointPositions(ctx, &pb.MoveToJointPositionsRequest{ Name: c.name, - Positions: positions, + Positions: jp, Extra: ext, }) return err } -func (c *client) JointPositions(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { +func (c *client) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { ext, err := protoutils.StructToStructPb(extra) if err != nil { return nil, err @@ -115,7 +121,7 @@ func (c *client) JointPositions(ctx context.Context, extra map[string]interface{ if err != nil { return nil, err } - return resp.Positions, nil + return referenceframe.InputsFromJointPositions(c.model, resp.Positions) } func (c *client) Stop(ctx context.Context, extra map[string]interface{}) error { @@ -135,23 +141,12 @@ func (c *client) ModelFrame() referenceframe.Model { } func (c *client) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { - if c.model == nil { - return nil, errArmClientModelNotValid - } - resp, err := c.JointPositions(ctx, nil) - if err != nil { - return nil, err - } - return c.model.InputFromProtobuf(resp), nil + return c.JointPositions(ctx, nil) } func (c *client) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { - if c.model == nil { - return errArmClientModelNotValid - } for _, goal := range inputSteps { - err := c.MoveToJointPositions(ctx, c.model.ProtobufFromInput(goal), nil) - if err != nil { + if err := c.MoveToJointPositions(ctx, goal, nil); err != nil { return err } } diff --git a/components/arm/client_test.go b/components/arm/client_test.go index e0191286aa5..a2e50585b54 100644 --- a/components/arm/client_test.go +++ b/components/arm/client_test.go @@ -6,7 +6,6 @@ import ( "testing" "github.com/golang/geo/r3" - componentpb "go.viam.com/api/component/arm/v1" robotpb "go.viam.com/api/robot/v1" "go.viam.com/test" "go.viam.com/utils/rpc" @@ -32,19 +31,19 @@ func TestClient(t *testing.T) { var ( capArmPos spatialmath.Pose - capArmJointPos *componentpb.JointPositions + capArmJointPos []referenceframe.Input extraOptions map[string]interface{} ) pos1 := spatialmath.NewPoseFromPoint(r3.Vector{X: 1, Y: 2, Z: 3}) - jointPos1 := &componentpb.JointPositions{Values: []float64{1.0, 2.0, 3.0}} + jointPos1 := []referenceframe.Input{{1.}, {2.}, {3.}} expectedGeometries := []spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{1, 2, 3}, "")} injectArm := &inject.Arm{} injectArm.EndPositionFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) { extraOptions = extra return pos1, nil } - injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) (*componentpb.JointPositions, error) { + injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { extraOptions = extra return jointPos1, nil } @@ -54,7 +53,7 @@ func TestClient(t *testing.T) { return nil } - injectArm.MoveToJointPositionsFunc = func(ctx context.Context, jp *componentpb.JointPositions, extra map[string]interface{}) error { + injectArm.MoveToJointPositionsFunc = func(ctx context.Context, jp []referenceframe.Input, extra map[string]interface{}) error { capArmJointPos = jp extraOptions = extra return nil @@ -64,22 +63,19 @@ func TestClient(t *testing.T) { return errStopUnimplemented } injectArm.ModelFrameFunc = func() referenceframe.Model { - data := []byte("{\"links\": [{\"parent\": \"world\"}]}") - model, err := referenceframe.UnmarshalModelJSON(data, "") - test.That(t, err, test.ShouldBeNil) - return model + return nil } injectArm.GeometriesFunc = func(ctx context.Context) ([]spatialmath.Geometry, error) { return expectedGeometries, nil } pos2 := spatialmath.NewPoseFromPoint(r3.Vector{X: 4, Y: 5, Z: 6}) - jointPos2 := &componentpb.JointPositions{Values: []float64{4.0, 5.0, 6.0}} + jointPos2 := []referenceframe.Input{{4.}, {5.}, {6.}} injectArm2 := &inject.Arm{} injectArm2.EndPositionFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) { return pos2, nil } - injectArm2.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) (*componentpb.JointPositions, error) { + injectArm2.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { return jointPos2, nil } injectArm2.MoveToPositionFunc = func(ctx context.Context, ap spatialmath.Pose, extra map[string]interface{}) error { @@ -87,7 +83,7 @@ func TestClient(t *testing.T) { return nil } - injectArm2.MoveToJointPositionsFunc = func(ctx context.Context, jp *componentpb.JointPositions, extra map[string]interface{}) error { + injectArm2.MoveToJointPositionsFunc = func(ctx context.Context, jp []referenceframe.Input, extra map[string]interface{}) error { capArmJointPos = jp return nil } @@ -95,10 +91,7 @@ func TestClient(t *testing.T) { return nil } injectArm2.ModelFrameFunc = func() referenceframe.Model { - data := []byte("{\"links\": [{\"parent\": \"world\"}]}") - model, err := referenceframe.UnmarshalModelJSON(data, "") - test.That(t, err, test.ShouldBeNil) - return model + return nil } armSvc, err := resource.NewAPIResourceCollection( @@ -157,7 +150,7 @@ func TestClient(t *testing.T) { jointPos, err := arm1Client.JointPositions(context.Background(), map[string]interface{}{"foo": "JointPositions"}) test.That(t, err, test.ShouldBeNil) - test.That(t, jointPos.String(), test.ShouldResemble, jointPos1.String()) + test.That(t, jointPos, test.ShouldResemble, jointPos1) test.That(t, extraOptions, test.ShouldResemble, map[string]interface{}{"foo": "JointPositions"}) err = arm1Client.MoveToPosition(context.Background(), pos2, map[string]interface{}{"foo": "MoveToPosition"}) @@ -168,7 +161,7 @@ func TestClient(t *testing.T) { err = arm1Client.MoveToJointPositions(context.Background(), jointPos2, map[string]interface{}{"foo": "MoveToJointPositions"}) test.That(t, err, test.ShouldBeNil) - test.That(t, capArmJointPos.String(), test.ShouldResemble, jointPos2.String()) + test.That(t, capArmJointPos, test.ShouldResemble, jointPos2) test.That(t, extraOptions, test.ShouldResemble, map[string]interface{}{"foo": "MoveToJointPositions"}) err = arm1Client.Stop(context.Background(), map[string]interface{}{"foo": "Stop"}) diff --git a/components/arm/collector.go b/components/arm/collector.go index 8df74a49970..6dd9a60c618 100644 --- a/components/arm/collector.go +++ b/components/arm/collector.go @@ -11,6 +11,7 @@ import ( "google.golang.org/protobuf/types/known/anypb" "go.viam.com/rdk/data" + "go.viam.com/rdk/referenceframe" ) type method int64 @@ -82,9 +83,11 @@ func newJointPositionsCollector(resource interface{}, params data.CollectorParam } return nil, data.FailedToReadErr(params.ComponentName, jointPositions.String(), err) } - return pb.GetJointPositionsResponse{ - Positions: v, - }, nil + jp, err := referenceframe.JointPositionsFromInputs(arm.ModelFrame(), v) + if err != nil { + return nil, data.FailedToReadErr(params.ComponentName, jointPositions.String(), err) + } + return pb.GetJointPositionsResponse{Positions: jp}, nil }) return data.NewCollector(cFunc, params) } diff --git a/components/arm/collectors_test.go b/components/arm/collectors_test.go index bd737b91ba2..1313a2a7186 100644 --- a/components/arm/collectors_test.go +++ b/components/arm/collectors_test.go @@ -14,6 +14,7 @@ import ( "go.viam.com/rdk/components/arm" "go.viam.com/rdk/data" "go.viam.com/rdk/logging" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" tu "go.viam.com/rdk/testutils" "go.viam.com/rdk/testutils/inject" @@ -25,7 +26,7 @@ const ( numRetries = 5 ) -var floatList = []float64{1.0, 2.0, 3.0} +var floatList = &pb.JointPositions{Values: []float64{1.0, 2.0, 3.0}} func TestCollectors(t *testing.T) { tests := []struct { @@ -51,11 +52,7 @@ func TestCollectors(t *testing.T) { { name: "Joint positions collector should write a list of positions", collector: arm.NewJointPositionsCollector, - expected: tu.ToProtoMapIgnoreOmitEmpty(pb.GetJointPositionsResponse{ - Positions: &pb.JointPositions{ - Values: floatList, - }, - }), + expected: tu.ToProtoMapIgnoreOmitEmpty(pb.GetJointPositionsResponse{Positions: floatList}), }, } @@ -93,10 +90,11 @@ func newArm() arm.Arm { a.EndPositionFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) { return spatialmath.NewPoseFromPoint(r3.Vector{X: 1, Y: 2, Z: 3}), nil } - a.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { - return &pb.JointPositions{ - Values: floatList, - }, nil + a.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { + return referenceframe.FloatsToInputs(referenceframe.JointPositionsToRadians(floatList)), nil + } + a.ModelFrameFunc = func() referenceframe.Model { + return nil } return a } diff --git a/components/arm/eva/eva.go b/components/arm/eva/eva.go index d5a69a21603..b32d0a6ca10 100644 --- a/components/arm/eva/eva.go +++ b/components/arm/eva/eva.go @@ -6,6 +6,7 @@ package eva import ( "bytes" "context" + // for embedding model file. _ "embed" "encoding/json" @@ -18,7 +19,6 @@ import ( "github.com/pkg/errors" "go.uber.org/multierr" - pb "go.viam.com/api/component/arm/v1" "go.viam.com/utils" "go.viam.com/rdk/components/arm" @@ -129,12 +129,12 @@ func NewEva(ctx context.Context, conf resource.Config, logger logging.Logger) (a return e, nil } -func (e *eva) JointPositions(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { +func (e *eva) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { data, err := e.DataSnapshot(ctx) if err != nil { - return &pb.JointPositions{}, err + return nil, err } - return referenceframe.JointPositionsFromRadians(data.ServosPosition), nil + return referenceframe.FloatsToInputs(data.ServosPosition), nil } // EndPosition computes and returns the current cartesian position. @@ -153,17 +153,15 @@ func (e *eva) MoveToPosition(ctx context.Context, pos spatialmath.Pose, extra ma return motion.MoveArm(ctx, e.logger, e, pos) } -func (e *eva) MoveToJointPositions(ctx context.Context, newPositions *pb.JointPositions, extra map[string]interface{}) error { +func (e *eva) MoveToJointPositions(ctx context.Context, newPositions []referenceframe.Input, extra map[string]interface{}) error { // check that joint positions are not out of bounds - inputs := e.ModelFrame().InputFromProtobuf(newPositions) - if err := arm.CheckDesiredJointPositions(ctx, e, inputs); err != nil { + if err := arm.CheckDesiredJointPositions(ctx, e, newPositions); err != nil { return err } ctx, done := e.opMgr.New(ctx) defer done() - radians := referenceframe.JointPositionsToRadians(newPositions) - + radians := referenceframe.InputsToFloats(newPositions) err := e.doMoveJoints(ctx, radians) if err == nil { return nil @@ -383,11 +381,7 @@ func (e *eva) ModelFrame() referenceframe.Model { } func (e *eva) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { - res, err := e.JointPositions(ctx, nil) - if err != nil { - return nil, err - } - return e.model.InputFromProtobuf(res), nil + return e.JointPositions(ctx, nil) } func (e *eva) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { @@ -395,7 +389,7 @@ func (e *eva) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Inp if err := arm.CheckDesiredJointPositions(ctx, e, goal); err != nil { return err } - err := e.MoveToJointPositions(ctx, e.model.ProtobufFromInput(goal), nil) + err := e.MoveToJointPositions(ctx, goal, nil) if err != nil { return err } diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index 38a3dac88cb..4f9a4433c90 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -8,7 +8,6 @@ import ( "sync" "github.com/pkg/errors" - pb "go.viam.com/api/component/arm/v1" "go.viam.com/rdk/components/arm" "go.viam.com/rdk/components/arm/eva" @@ -175,26 +174,25 @@ func (a *Arm) MoveToPosition(ctx context.Context, pose spatialmath.Pose, extra m } // MoveToJointPositions sets the joints. -func (a *Arm) MoveToJointPositions(ctx context.Context, joints *pb.JointPositions, extra map[string]interface{}) error { - inputs := a.model.InputFromProtobuf(joints) - if err := arm.CheckDesiredJointPositions(ctx, a, inputs); err != nil { +func (a *Arm) MoveToJointPositions(ctx context.Context, joints []referenceframe.Input, extra map[string]interface{}) error { + if err := arm.CheckDesiredJointPositions(ctx, a, joints); err != nil { return err } a.mu.RLock() defer a.mu.RUnlock() - _, err := a.model.Transform(inputs) + _, err := a.model.Transform(joints) if err != nil { return err } - copy(a.joints, inputs) + copy(a.joints, joints) return nil } // JointPositions returns joints. -func (a *Arm) JointPositions(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { +func (a *Arm) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { a.mu.RLock() defer a.mu.RUnlock() - return a.model.ProtobufFromInput(a.joints), nil + return a.joints, nil } // Stop doesn't do anything for a fake arm. @@ -217,11 +215,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 { - a.mu.RLock() - positionDegs := a.model.ProtobufFromInput(goal) - a.mu.RUnlock() - err := a.MoveToJointPositions(ctx, positionDegs, nil) - if err != nil { + if err := a.MoveToJointPositions(ctx, goal, nil); err != nil { return err } } diff --git a/components/arm/fake/fake_test.go b/components/arm/fake/fake_test.go index 4a541aff7b5..c5e4874ed99 100644 --- a/components/arm/fake/fake_test.go +++ b/components/arm/fake/fake_test.go @@ -2,9 +2,9 @@ package fake import ( "context" + "math" "testing" - pb "go.viam.com/api/component/arm/v1" "go.viam.com/test" "go.viam.com/rdk/logging" @@ -115,13 +115,13 @@ func TestJointPositions(t *testing.T) { // Round trip test for MoveToJointPositions -> JointPositions arm, err := NewArm(ctx, nil, cfg, logger) test.That(t, err, test.ShouldBeNil) - samplePositions := &pb.JointPositions{Values: []float64{0, 30, 60, 90, 60, 30}} + samplePositions := []referenceframe.Input{{0}, {math.Pi}, {-math.Pi}, {0}, {math.Pi}, {-math.Pi}} test.That(t, arm.MoveToJointPositions(ctx, samplePositions, nil), test.ShouldBeNil) positions, err := arm.JointPositions(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, len(positions.Values), test.ShouldEqual, len(samplePositions.Values)) - for i := range samplePositions.Values { - test.That(t, positions.Values[i], test.ShouldAlmostEqual, samplePositions.Values[i]) + test.That(t, len(positions), test.ShouldEqual, len(samplePositions)) + for i := range samplePositions { + test.That(t, positions[i], test.ShouldResemble, samplePositions[i]) } // Round trip test for GoToInputs -> CurrentInputs diff --git a/components/arm/server.go b/components/arm/server.go index aba3d26af95..72d65042f07 100644 --- a/components/arm/server.go +++ b/components/arm/server.go @@ -12,6 +12,7 @@ import ( "go.viam.com/rdk/operation" "go.viam.com/rdk/protoutils" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/referenceframe/urdf" "go.viam.com/rdk/resource" "go.viam.com/rdk/spatialmath" @@ -55,10 +56,7 @@ func (s *serviceServer) GetEndPosition( } // GetJointPositions gets the current joint position of an arm of the underlying robot. -func (s *serviceServer) GetJointPositions( - ctx context.Context, - req *pb.GetJointPositionsRequest, -) (*pb.GetJointPositionsResponse, error) { +func (s *serviceServer) GetJointPositions(ctx context.Context, req *pb.GetJointPositionsRequest) (*pb.GetJointPositionsResponse, error) { arm, err := s.coll.Resource(req.Name) if err != nil { return nil, err @@ -67,13 +65,11 @@ func (s *serviceServer) GetJointPositions( if err != nil { return nil, err } - // Have a default empty joint position object in case the position returned is nil, - // this guards against nil objects being transferred over the wire. - convertedPos := &pb.JointPositions{Values: []float64{}} - if pos != nil { - convertedPos.Values = pos.Values + jp, err := referenceframe.JointPositionsFromInputs(arm.ModelFrame(), pos) + if err != nil { + return nil, err } - return &pb.GetJointPositionsResponse{Positions: convertedPos}, nil + return &pb.GetJointPositionsResponse{Positions: jp}, nil } // MoveToPosition returns the position of the arm specified. @@ -100,7 +96,11 @@ func (s *serviceServer) MoveToJointPositions( if err != nil { return nil, err } - return &pb.MoveToJointPositionsResponse{}, arm.MoveToJointPositions(ctx, req.Positions, req.Extra.AsMap()) + inputs, err := referenceframe.InputsFromJointPositions(arm.ModelFrame(), req.Positions) + if err != nil { + return nil, err + } + return &pb.MoveToJointPositionsResponse{}, arm.MoveToJointPositions(ctx, inputs, req.Extra.AsMap()) } // Stop stops the arm specified. diff --git a/components/arm/server_test.go b/components/arm/server_test.go index e087ba02fd8..daca3d95a1f 100644 --- a/components/arm/server_test.go +++ b/components/arm/server_test.go @@ -49,19 +49,19 @@ func TestServer(t *testing.T) { var ( capArmPos spatialmath.Pose - capArmJointPos *pb.JointPositions + capArmJointPos []referenceframe.Input extraOptions map[string]interface{} ) pose1 := spatialmath.NewPoseFromPoint(r3.Vector{X: 1, Y: 2, Z: 3}) - positionDegs1 := &pb.JointPositions{Values: []float64{1.0, 2.0, 3.0}} + positions := []float64{1., 2., 3., 1., 2., 3.} injectArm.EndPositionFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) { extraOptions = extra return pose1, nil } - injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { + injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { extraOptions = extra - return positionDegs1, nil + return referenceframe.FloatsToInputs(positions), nil } injectArm.MoveToPositionFunc = func(ctx context.Context, ap spatialmath.Pose, extra map[string]interface{}) error { capArmPos = ap @@ -69,7 +69,7 @@ func TestServer(t *testing.T) { return nil } - injectArm.MoveToJointPositionsFunc = func(ctx context.Context, jp *pb.JointPositions, extra map[string]interface{}) error { + injectArm.MoveToJointPositionsFunc = func(ctx context.Context, jp []referenceframe.Input, extra map[string]interface{}) error { capArmJointPos = jp extraOptions = extra return nil @@ -87,11 +87,11 @@ func TestServer(t *testing.T) { } pose2 := &commonpb.Pose{X: 4, Y: 5, Z: 6} - positionDegs2 := &pb.JointPositions{Values: []float64{4.0, 5.0, 6.0}} + positionDegs2 := &pb.JointPositions{Values: []float64{4.0, 5.0, 6.0, 4.0, 5.0, 6.0}} injectArm2.EndPositionFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) { return nil, errGetPoseFailed } - injectArm2.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { + injectArm2.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { return nil, errGetJointsFailed } injectArm2.MoveToPositionFunc = func(ctx context.Context, ap spatialmath.Pose, extra map[string]interface{}) error { @@ -99,7 +99,7 @@ func TestServer(t *testing.T) { return errMoveToPositionFailed } - injectArm2.MoveToJointPositionsFunc = func(ctx context.Context, jp *pb.JointPositions, extra map[string]interface{}) error { + injectArm2.MoveToJointPositionsFunc = func(ctx context.Context, jp []referenceframe.Input, extra map[string]interface{}) error { capArmJointPos = jp return errMoveToJointPositionFailed } @@ -166,7 +166,7 @@ func TestServer(t *testing.T) { test.That(t, err, test.ShouldBeNil) resp, err := armServer.GetJointPositions(context.Background(), &pb.GetJointPositionsRequest{Name: testArmName, Extra: ext}) test.That(t, err, test.ShouldBeNil) - test.That(t, resp.Positions.String(), test.ShouldResemble, positionDegs1.String()) + test.That(t, referenceframe.JointPositionsToRadians(resp.Positions), test.ShouldResemble, positions) test.That(t, extraOptions, test.ShouldResemble, map[string]interface{}{"foo": "JointPositions"}) _, err = armServer.GetJointPositions(context.Background(), &pb.GetJointPositionsRequest{Name: failArmName}) @@ -175,12 +175,11 @@ func TestServer(t *testing.T) { // Redefine JointPositionsFunc to test nil return. //nolint: nilnil - injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { + injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { return nil, nil } - resp, err = armServer.GetJointPositions(context.Background(), &pb.GetJointPositionsRequest{Name: testArmName}) - test.That(t, err, test.ShouldBeNil) - test.That(t, resp.Positions.Values, test.ShouldResemble, []float64{}) + _, err = armServer.GetJointPositions(context.Background(), &pb.GetJointPositionsRequest{Name: testArmName}) + test.That(t, err.Error(), test.ShouldResemble, referenceframe.NewIncorrectDoFError(0, len(positions)).Error()) }) t.Run("move to joint position", func(t *testing.T) { @@ -198,16 +197,17 @@ func TestServer(t *testing.T) { &pb.MoveToJointPositionsRequest{Name: testArmName, Positions: positionDegs2, Extra: ext}, ) test.That(t, err, test.ShouldBeNil) - test.That(t, capArmJointPos.String(), test.ShouldResemble, positionDegs2.String()) + positionsRads2, err := referenceframe.InputsFromJointPositions(nil, positionDegs2) + test.That(t, err, test.ShouldBeNil) + test.That(t, capArmJointPos, test.ShouldResemble, positionsRads2) test.That(t, extraOptions, test.ShouldResemble, map[string]interface{}{"foo": "MoveToJointPositions"}) _, err = armServer.MoveToJointPositions( context.Background(), - &pb.MoveToJointPositionsRequest{Name: failArmName, Positions: positionDegs1}, + &pb.MoveToJointPositionsRequest{Name: failArmName, Positions: &pb.JointPositions{Values: positions}}, ) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, errMoveToJointPositionFailed.Error()) - test.That(t, capArmJointPos.String(), test.ShouldResemble, positionDegs1.String()) }) t.Run("get kinematics", func(t *testing.T) { diff --git a/components/arm/universalrobots/ur.go b/components/arm/universalrobots/ur.go index 6a706a0a91a..e81eaae6d7b 100644 --- a/components/arm/universalrobots/ur.go +++ b/components/arm/universalrobots/ur.go @@ -4,6 +4,7 @@ package universalrobots import ( "bufio" "context" + // for embedding model file. _ "embed" "encoding/binary" @@ -19,7 +20,6 @@ import ( "github.com/pkg/errors" "go.uber.org/multierr" - pb "go.viam.com/api/component/arm/v1" goutils "go.viam.com/utils" "go.viam.com/rdk/components/arm" @@ -332,7 +332,7 @@ func (ua *urArm) getState() (robotState, error) { } // JointPositions gets the current joint positions of the UR arm. -func (ua *urArm) JointPositions(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { +func (ua *urArm) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { radians := []float64{} state, err := ua.getState() if err != nil { @@ -341,7 +341,7 @@ func (ua *urArm) JointPositions(ctx context.Context, extra map[string]interface{ for _, j := range state.Joints { radians = append(radians, j.Qactual) } - return referenceframe.JointPositionsFromRadians(radians), nil + return referenceframe.FloatsToInputs(radians), nil } // EndPosition computes and returns the current cartesian position. @@ -376,13 +376,12 @@ func (ua *urArm) MoveToPosition(ctx context.Context, pos spatialmath.Pose, extra } // MoveToJointPositions moves the UR arm to the specified joint positions. -func (ua *urArm) MoveToJointPositions(ctx context.Context, joints *pb.JointPositions, extra map[string]interface{}) error { +func (ua *urArm) MoveToJointPositions(ctx context.Context, joints []referenceframe.Input, extra map[string]interface{}) error { // check that joint positions are not out of bounds - inputs := ua.ModelFrame().InputFromProtobuf(joints) - if err := arm.CheckDesiredJointPositions(ctx, ua, inputs); err != nil { + if err := arm.CheckDesiredJointPositions(ctx, ua, joints); err != nil { return err } - return ua.moveToJointPositionRadians(ctx, referenceframe.JointPositionsToRadians(joints)) + return ua.moveToJointPositionRadians(ctx, referenceframe.InputsToFloats(joints)) } // Stop stops the arm with some deceleration. @@ -494,11 +493,7 @@ func (ua *urArm) moveToJointPositionRadians(ctx context.Context, radians []float // CurrentInputs returns the current Inputs of the UR arm. func (ua *urArm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { - res, err := ua.JointPositions(ctx, nil) - if err != nil { - return nil, err - } - return ua.model.InputFromProtobuf(res), nil + return ua.JointPositions(ctx, nil) } // GoToInputs moves the UR arm to the Inputs specified. @@ -508,7 +503,7 @@ func (ua *urArm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe. if err := arm.CheckDesiredJointPositions(ctx, ua, goal); err != nil { return err } - err := ua.MoveToJointPositions(ctx, ua.model.ProtobufFromInput(goal), nil) + err := ua.MoveToJointPositions(ctx, goal, nil) if err != nil { return err } diff --git a/components/arm/wrapper/wrapper.go b/components/arm/wrapper/wrapper.go index a1f0fcb1f23..ca4d7815c80 100644 --- a/components/arm/wrapper/wrapper.go +++ b/components/arm/wrapper/wrapper.go @@ -7,8 +7,6 @@ import ( "strings" "sync" - pb "go.viam.com/api/component/arm/v1" - "go.viam.com/rdk/components/arm" "go.viam.com/rdk/logging" "go.viam.com/rdk/operation" @@ -124,10 +122,9 @@ func (wrapper *Arm) MoveToPosition(ctx context.Context, pos spatialmath.Pose, ex } // MoveToJointPositions sets the joints. -func (wrapper *Arm) MoveToJointPositions(ctx context.Context, joints *pb.JointPositions, extra map[string]interface{}) error { +func (wrapper *Arm) MoveToJointPositions(ctx context.Context, joints []referenceframe.Input, extra map[string]interface{}) error { // check that joint positions are not out of bounds - inputs := wrapper.model.InputFromProtobuf(joints) - if err := arm.CheckDesiredJointPositions(ctx, wrapper, inputs); err != nil { + if err := arm.CheckDesiredJointPositions(ctx, wrapper, joints); err != nil { return err } ctx, done := wrapper.opMgr.New(ctx) @@ -139,7 +136,7 @@ func (wrapper *Arm) MoveToJointPositions(ctx context.Context, joints *pb.JointPo } // JointPositions returns the set joints. -func (wrapper *Arm) JointPositions(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { +func (wrapper *Arm) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { wrapper.mu.RLock() defer wrapper.mu.RUnlock() @@ -169,11 +166,7 @@ func (wrapper *Arm) IsMoving(ctx context.Context) (bool, error) { func (wrapper *Arm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { wrapper.mu.RLock() defer wrapper.mu.RUnlock() - res, err := wrapper.actual.JointPositions(ctx, nil) - if err != nil { - return nil, err - } - return wrapper.model.InputFromProtobuf(res), nil + return wrapper.actual.JointPositions(ctx, nil) } // GoToInputs moves the arm to the specified goal inputs. @@ -183,7 +176,7 @@ func (wrapper *Arm) GoToInputs(ctx context.Context, inputSteps ...[]referencefra if err := arm.CheckDesiredJointPositions(ctx, wrapper, goal); err != nil { return err } - err := wrapper.MoveToJointPositions(ctx, wrapper.model.ProtobufFromInput(goal), nil) + err := wrapper.MoveToJointPositions(ctx, goal, nil) if err != nil { return err } diff --git a/components/arm/xarm/xarm.go b/components/arm/xarm/xarm.go index 91185af1069..01388beed57 100644 --- a/components/arm/xarm/xarm.go +++ b/components/arm/xarm/xarm.go @@ -191,11 +191,7 @@ func (x *xArm) Reconfigure(ctx context.Context, deps resource.Dependencies, conf } func (x *xArm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { - res, err := x.JointPositions(ctx, nil) - if err != nil { - return nil, err - } - return x.model.InputFromProtobuf(res), nil + return x.JointPositions(ctx, nil) } func (x *xArm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { @@ -209,8 +205,7 @@ func (x *xArm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.In if err != nil { return err } - from := x.model.InputFromProtobuf(curPos) - armRawSteps, err := x.createRawJointSteps(from, inputSteps) + armRawSteps, err := x.createRawJointSteps(curPos, inputSteps) if err != nil { return err } diff --git a/components/arm/xarm/xarm_comm.go b/components/arm/xarm/xarm_comm.go index 0356a90f0ac..a2592d294a3 100644 --- a/components/arm/xarm/xarm_comm.go +++ b/components/arm/xarm/xarm_comm.go @@ -8,7 +8,6 @@ import ( "time" "go.uber.org/multierr" - pb "go.viam.com/api/component/arm/v1" "go.viam.com/utils" "go.viam.com/rdk/referenceframe" @@ -353,10 +352,10 @@ func (x *xArm) Close(ctx context.Context) error { } // MoveToJointPositions moves the arm to the requested joint positions. -func (x *xArm) MoveToJointPositions(ctx context.Context, newPositions *pb.JointPositions, extra map[string]interface{}) error { +func (x *xArm) MoveToJointPositions(ctx context.Context, newPositions []referenceframe.Input, extra map[string]interface{}) error { ctx, done := x.opMgr.New(ctx) defer done() - return x.GoToInputs(ctx, x.model.InputFromProtobuf(newPositions)) + return x.GoToInputs(ctx, newPositions) } // Using the configured moveHz, joint speed, and joint acceleration, create the series of joint positions for the arm to follow, @@ -553,20 +552,19 @@ func (x *xArm) MoveToPosition(ctx context.Context, pos spatialmath.Pose, extra m } // JointPositions returns the current positions of all joints. -func (x *xArm) JointPositions(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { +func (x *xArm) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { c := x.newCmd(regMap["JointPos"]) jData, err := x.send(ctx, c, true) if err != nil { - return &pb.JointPositions{}, err + return nil, err } var radians []float64 for i := 0; i < x.dof; i++ { idx := i*4 + 1 radians = append(radians, float64(rutils.Float32FromBytesLE((jData.params[idx : idx+4])))) } - - return referenceframe.JointPositionsFromRadians(radians), nil + return referenceframe.FloatsToInputs(radians), nil } // Stop stops the xArm but also reinitializes the arm so it can take commands again. diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index df25b21ed24..105538f9d42 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -115,10 +115,10 @@ func (req *PlanRequest) validatePlanRequest() error { return errors.Errorf("%s does not have a start configuration", req.Frame.Name()) } if frameDOF != len(seedMap) { - return frame.NewIncorrectInputLengthError(len(seedMap), len(req.Frame.DoF())) + return frame.NewIncorrectDoFError(len(seedMap), len(req.Frame.DoF())) } } else if ok && frameDOF != len(seedMap) { - return frame.NewIncorrectInputLengthError(len(seedMap), len(req.Frame.DoF())) + return frame.NewIncorrectDoFError(len(seedMap), len(req.Frame.DoF())) } return nil diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index e6c438d5b06..a4f260c7495 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -180,7 +180,7 @@ func (sf *solverFrame) Name() string { // Transform returns the pose between the two frames of this solver for a given set of inputs. func (sf *solverFrame) Transform(inputs []frame.Input) (spatial.Pose, error) { if len(inputs) != len(sf.DoF()) { - return nil, frame.NewIncorrectInputLengthError(len(inputs), len(sf.DoF())) + return nil, frame.NewIncorrectDoFError(len(inputs), len(sf.DoF())) } pf := frame.NewPoseInFrame(sf.solveFrameName, spatial.NewZeroPose()) solveName := sf.goalFrameName @@ -197,10 +197,10 @@ func (sf *solverFrame) Transform(inputs []frame.Input) (spatial.Pose, error) { // Interpolate interpolates the given amount between the two sets of inputs. func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame.Input, error) { if len(from) != len(sf.DoF()) { - return nil, frame.NewIncorrectInputLengthError(len(from), len(sf.DoF())) + return nil, frame.NewIncorrectDoFError(len(from), len(sf.DoF())) } if len(to) != len(sf.DoF()) { - return nil, frame.NewIncorrectInputLengthError(len(to), len(sf.DoF())) + return nil, frame.NewIncorrectDoFError(len(to), len(sf.DoF())) } interp := make([]frame.Input, 0, len(to)) posIdx := 0 @@ -250,7 +250,7 @@ func (sf *solverFrame) ProtobufFromInput(input []frame.Input) *pb.JointPositions // geometries in the solverFrame in the reference frame of the World frame. func (sf *solverFrame) Geometries(inputs []frame.Input) (*frame.GeometriesInFrame, error) { if len(inputs) != len(sf.DoF()) { - return nil, frame.NewIncorrectInputLengthError(len(inputs), len(sf.DoF())) + return nil, frame.NewIncorrectDoFError(len(inputs), len(sf.DoF())) } var errAll error inputMap := sf.sliceToMap(inputs) diff --git a/motionplan/tpspace/ptgDiffDrive.go b/motionplan/tpspace/ptgDiffDrive.go index a9008524c44..a6d26d06682 100644 --- a/motionplan/tpspace/ptgDiffDrive.go +++ b/motionplan/tpspace/ptgDiffDrive.go @@ -38,7 +38,7 @@ func (ptg *ptgDiffDrive) Velocities(alpha, dist float64) (float64, float64, erro // rotation. For distance, dist is equal to the number of radians rotated plus the number of millimeters of straight motion. func (ptg *ptgDiffDrive) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { if len(inputs) != 2 { - return nil, referenceframe.NewIncorrectInputLengthError(len(inputs), 2) + return nil, referenceframe.NewIncorrectDoFError(len(inputs), 2) } alpha := inputs[0].Value dist := inputs[1].Value diff --git a/motionplan/tpspace/ptgGroupFrame.go b/motionplan/tpspace/ptgGroupFrame.go index ced465139c9..fefd580f73c 100644 --- a/motionplan/tpspace/ptgGroupFrame.go +++ b/motionplan/tpspace/ptgGroupFrame.go @@ -287,7 +287,7 @@ func (pf *ptgGroupFrame) PTGSolvers() []PTGSolver { func (pf *ptgGroupFrame) validInputs(inputs []referenceframe.Input) error { var errAll error if len(inputs) != len(pf.limits) { - return referenceframe.NewIncorrectInputLengthError(len(inputs), len(pf.limits)) + return referenceframe.NewIncorrectDoFError(len(inputs), len(pf.limits)) } for i := 0; i < len(pf.limits); i++ { if inputs[i].Value < pf.limits[i].Min || inputs[i].Value > pf.limits[i].Max { diff --git a/motionplan/tpspace/ptgIKFrame.go b/motionplan/tpspace/ptgIKFrame.go index ec8305f7c03..94131a32db9 100644 --- a/motionplan/tpspace/ptgIKFrame.go +++ b/motionplan/tpspace/ptgIKFrame.go @@ -57,7 +57,7 @@ func (pf *ptgIKFrame) Geometries(inputs []referenceframe.Input) (*referenceframe func (pf *ptgIKFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { if len(inputs) != len(pf.DoF()) && len(inputs) != 2 { // We also want to always support 2 inputs - return nil, referenceframe.NewIncorrectInputLengthError(len(inputs), len(pf.DoF())) + return nil, referenceframe.NewIncorrectDoFError(len(inputs), len(pf.DoF())) } p1 := spatialmath.NewZeroPose() for i := 0; i < len(inputs); i += 2 { diff --git a/referenceframe/errors.go b/referenceframe/errors.go index 113e84947b7..4d1549d1f34 100644 --- a/referenceframe/errors.go +++ b/referenceframe/errors.go @@ -44,9 +44,9 @@ func NewFrameAlreadyExistsError(frameName string) error { return errors.Errorf("frame with name %q already in frame system", frameName) } -// NewIncorrectInputLengthError returns an error indicating that the length of the Input array does not match the DoF of the frame. -func NewIncorrectInputLengthError(actual, expected int) error { - return errors.Errorf("number of inputs does not match frame DoF, expected %d but got %d", expected, actual) +// NewIncorrectDoFError returns an error indicating that the length of the array does not match the DoF of the frame. +func NewIncorrectDoFError(actual, expected int) error { + return errors.Errorf("array length does not match frame DoF, expected %d but got %d", expected, actual) } // NewUnsupportedJointTypeError returns an error indicating that a given joint type is not supported by current model parsing. diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 4bbf36726e7..5f12bd94cdc 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -39,7 +39,7 @@ func RestrictedRandomFrameInputs(m Frame, rSeed *rand.Rand, restrictionPercent f } dof := m.DoF() if len(reference) != len(dof) { - return nil, NewIncorrectInputLengthError(len(reference), len(dof)) + return nil, NewIncorrectDoFError(len(reference), len(dof)) } pos := make([]Input, 0, len(dof)) for i, limit := range dof { @@ -151,7 +151,7 @@ func (bf *baseFrame) Interpolate(from, to []Input, by float64) ([]Input, error) func (bf *baseFrame) validInputs(inputs []Input) error { var errAll error if len(inputs) != len(bf.limits) { - return NewIncorrectInputLengthError(len(inputs), len(bf.limits)) + return NewIncorrectDoFError(len(inputs), len(bf.limits)) } for i := 0; i < len(bf.limits); i++ { if inputs[i].Value < bf.limits[i].Min || inputs[i].Value > bf.limits[i].Max { @@ -181,7 +181,7 @@ func (sf *tailGeometryStaticFrame) Geometries(input []Input) (*GeometriesInFrame return NewGeometriesInFrame(sf.Name(), nil), nil } if len(input) != 0 { - return nil, NewIncorrectInputLengthError(len(input), 0) + return nil, NewIncorrectDoFError(len(input), 0) } newGeom := sf.geometry.Transform(sf.transform) if newGeom.Label() == "" { @@ -242,7 +242,7 @@ func NewStaticFrameWithGeometry(name string, pose spatial.Pose, geometry spatial // Transform returns the pose associated with this static referenceframe. func (sf *staticFrame) Transform(input []Input) (spatial.Pose, error) { if len(input) != 0 { - return nil, NewIncorrectInputLengthError(len(input), 0) + return nil, NewIncorrectDoFError(len(input), 0) } return sf.transform, nil } @@ -263,7 +263,7 @@ func (sf *staticFrame) Geometries(input []Input) (*GeometriesInFrame, error) { return NewGeometriesInFrame(sf.Name(), nil), nil } if len(input) != 0 { - return nil, NewIncorrectInputLengthError(len(input), 0) + return nil, NewIncorrectDoFError(len(input), 0) } newGeom := sf.geometry.Transform(spatial.NewZeroPose()) if newGeom.Label() == "" { @@ -489,10 +489,10 @@ func (pf *poseFrame) Transform(inputs []Input) (spatial.Pose, error) { // Interpolate interpolates the given amount between the two sets of inputs. func (pf *poseFrame) Interpolate(from, to []Input, by float64) ([]Input, error) { if err := pf.baseFrame.validInputs(from); err != nil { - return nil, NewIncorrectInputLengthError(len(from), len(pf.DoF())) + return nil, NewIncorrectDoFError(len(from), len(pf.DoF())) } if err := pf.baseFrame.validInputs(to); err != nil { - return nil, NewIncorrectInputLengthError(len(to), len(pf.DoF())) + return nil, NewIncorrectDoFError(len(to), len(pf.DoF())) } fromPose, err := pf.Transform(from) if err != nil { diff --git a/referenceframe/primitives.go b/referenceframe/input.go similarity index 73% rename from referenceframe/primitives.go rename to referenceframe/input.go index c5ced88a77b..467532be170 100644 --- a/referenceframe/primitives.go +++ b/referenceframe/input.go @@ -10,13 +10,35 @@ import ( "go.viam.com/rdk/utils" ) -// Input wraps the input to a mutable frame, e.g. a joint angle or a gantry position. Revolute inputs should be in -// radians. Prismatic inputs should be in mm. -// TODO: Determine what more this needs, or eschew in favor of raw float64s if nothing needed. +// Input wraps the input to a mutable frame, e.g. a joint angle or a gantry position. +// - revolute inputs should be in radians. +// - prismatic inputs should be in mm. type Input struct { Value float64 } +func JointPositionsFromInputs(f Frame, inputs []Input) (*pb.JointPositions, error) { + if f == nil { + // if a frame is not provided, we will assume all inputs are specified in degrees and need to be converted to radians + return JointPositionsFromRadians(InputsToFloats(inputs)), nil + } + if len(f.DoF()) != len(inputs) { + return nil, NewIncorrectDoFError(len(inputs), len(f.DoF())) + } + return f.ProtobufFromInput(inputs), nil +} + +func InputsFromJointPositions(f Frame, jp *pb.JointPositions) ([]Input, error) { + if f == nil { + // if a frame is not provided, we will assume all inputs are specified in degrees and need to be converted to radians + return FloatsToInputs(JointPositionsToRadians(jp)), nil + } + if len(f.DoF()) != len(jp.Values) { + return nil, NewIncorrectDoFError(len(jp.Values), len(f.DoF())) + } + return f.InputFromProtobuf(jp), nil +} + // FloatsToInputs wraps a slice of floats in Inputs. func FloatsToInputs(floats []float64) []Input { inputs := make([]Input, len(floats)) diff --git a/referenceframe/primitives_test.go b/referenceframe/input_test.go similarity index 100% rename from referenceframe/primitives_test.go rename to referenceframe/input_test.go diff --git a/referenceframe/model.go b/referenceframe/model.go index 3cbf9de7e42..be69a832706 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -210,7 +210,7 @@ func (m *SimpleModel) ModelPieceFrames(inputs []Input) (map[string]Frame, error) // between quaternions and OV are not needed. func (m *SimpleModel) inputsToFrames(inputs []Input, collectAll bool) ([]*staticFrame, error) { if len(m.DoF()) != len(inputs) { - return nil, NewIncorrectInputLengthError(len(inputs), len(m.DoF())) + return nil, NewIncorrectDoFError(len(inputs), len(m.DoF())) } var err error poses := make([]*staticFrame, 0, len(m.OrdTransforms)) diff --git a/referenceframe/model_test.go b/referenceframe/model_test.go index 686967c846d..6bda57de026 100644 --- a/referenceframe/model_test.go +++ b/referenceframe/model_test.go @@ -75,12 +75,12 @@ func TestIncorrectInputs(t *testing.T) { // test incorrect number of inputs pose, err := m.Transform(make([]Input, dof+1)) test.That(t, pose, test.ShouldBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, NewIncorrectInputLengthError(dof+1, dof).Error()) + test.That(t, err.Error(), test.ShouldContainSubstring, NewIncorrectDoFError(dof+1, dof).Error()) // test incorrect number of inputs to Geometries gf, err := m.Geometries(make([]Input, dof-1)) test.That(t, gf, test.ShouldBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, NewIncorrectInputLengthError(dof-1, dof).Error()) + test.That(t, err.Error(), test.ShouldContainSubstring, NewIncorrectDoFError(dof-1, dof).Error()) } func TestModelGeometries(t *testing.T) { diff --git a/robot/client/client_test.go b/robot/client/client_test.go index 776900d379a..b452af0049f 100644 --- a/robot/client/client_test.go +++ b/robot/client/client_test.go @@ -495,7 +495,7 @@ func TestStatusClient(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "not found") - err = arm1.MoveToJointPositions(context.Background(), &armpb.JointPositions{Values: []float64{1}}, nil) + err = arm1.MoveToJointPositions(context.Background(), []referenceframe.Input{}, nil) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "not found") @@ -563,7 +563,7 @@ func TestStatusClient(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "not found") - err = resource1.(arm.Arm).MoveToJointPositions(context.Background(), &armpb.JointPositions{Values: []float64{1}}, nil) + err = resource1.(arm.Arm).MoveToJointPositions(context.Background(), []referenceframe.Input{}, nil) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "not found") diff --git a/robot/impl/local_robot_test.go b/robot/impl/local_robot_test.go index b8acda30b62..e9743f5317a 100644 --- a/robot/impl/local_robot_test.go +++ b/robot/impl/local_robot_test.go @@ -20,6 +20,7 @@ import ( "github.com/golang/geo/r3" "go.mongodb.org/mongo-driver/bson/primitive" "go.uber.org/zap" + // registers all components. commonpb "go.viam.com/api/common/v1" armpb "go.viam.com/api/component/arm/v1" @@ -1218,8 +1219,7 @@ func TestGetRemoteResourceAndGrandFather(t *testing.T) { test.That(t, err, test.ShouldBeNil) r0Arm, ok := r0arm1.(arm.Arm) test.That(t, ok, test.ShouldBeTrue) - tPos := referenceframe.JointPositionsFromRadians([]float64{math.Pi}) - err = r0Arm.MoveToJointPositions(context.Background(), tPos, nil) + err = r0Arm.MoveToJointPositions(context.Background(), []referenceframe.Input{{math.Pi}}, nil) test.That(t, err, test.ShouldBeNil) p0Arm1, err := r0Arm.JointPositions(context.Background(), nil) test.That(t, err, test.ShouldBeNil) @@ -1269,7 +1269,7 @@ func TestGetRemoteResourceAndGrandFather(t *testing.T) { test.That(t, ok, test.ShouldBeTrue) pos, err := rrArm1.JointPositions(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, pos.Values, test.ShouldResemble, p0Arm1.Values) + test.That(t, pos, test.ShouldResemble, p0Arm1) arm1, err = r.ResourceByName(arm.Named("arm1")) test.That(t, err, test.ShouldBeNil) @@ -1277,7 +1277,7 @@ func TestGetRemoteResourceAndGrandFather(t *testing.T) { test.That(t, ok, test.ShouldBeTrue) pos, err = rrArm1.JointPositions(ctx, nil) test.That(t, err, test.ShouldBeNil) - test.That(t, pos.Values, test.ShouldResemble, p0Arm1.Values) + test.That(t, pos, test.ShouldResemble, p0Arm1) _, err = r.ResourceByName(arm.Named("remote:foo:pieceArm")) test.That(t, err, test.ShouldBeNil) diff --git a/services/datamanager/builtin/builtin_test.go b/services/datamanager/builtin/builtin_test.go index ba1c59848c7..9a0de0b5adb 100644 --- a/services/datamanager/builtin/builtin_test.go +++ b/services/datamanager/builtin/builtin_test.go @@ -17,7 +17,6 @@ import ( "github.com/benbjohnson/clock" "github.com/pkg/errors" v1 "go.viam.com/api/app/datasync/v1" - v1Arm "go.viam.com/api/component/arm/v1" "go.viam.com/test" goutils "go.viam.com/utils" "go.viam.com/utils/rpc" @@ -33,6 +32,7 @@ import ( "go.viam.com/rdk/internal/cloud" cloudinject "go.viam.com/rdk/internal/testutils/inject" "go.viam.com/rdk/logging" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/datamanager" "go.viam.com/rdk/services/datamanager/builtin/capture" @@ -284,8 +284,8 @@ func TestFileDeletion(t *testing.T) { JointPositionsFunc: func( ctx context.Context, extra map[string]interface{}, - ) (*v1Arm.JointPositions, error) { - return &v1Arm.JointPositions{Values: []float64{1.0, 2.0, 3.0, 4.0}}, nil + ) ([]referenceframe.Input, error) { + return referenceframe.FloatsToInputs([]float64{1.0, 2.0, 3.0, 4.0}), nil }, }, gantry.Named("gantry1"): &inject.Gantry{ diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 5f1eb3a2a49..62ab422c1ef 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -39,7 +39,7 @@ func (wf *wrapperFrame) Name() string { // Transform returns the associated pose given a list of inputs. func (wf *wrapperFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { if len(inputs) != len(wf.DoF()) { - return nil, referenceframe.NewIncorrectInputLengthError(len(inputs), len(wf.DoF())) + return nil, referenceframe.NewIncorrectDoFError(len(inputs), len(wf.DoF())) } // executionFramePose is our pose in a given set of plan.Trajectory inputs executionFramePose, err := wf.executionFrame.Transform(inputs[:len(wf.executionFrame.DoF())]) @@ -58,10 +58,10 @@ func (wf *wrapperFrame) Transform(inputs []referenceframe.Input) (spatialmath.Po // Interpolate interpolates the given amount between the two sets of inputs. func (wf *wrapperFrame) Interpolate(from, to []referenceframe.Input, by float64) ([]referenceframe.Input, error) { if len(from) != len(wf.DoF()) { - return nil, referenceframe.NewIncorrectInputLengthError(len(from), len(wf.DoF())) + return nil, referenceframe.NewIncorrectDoFError(len(from), len(wf.DoF())) } if len(to) != len(wf.DoF()) { - return nil, referenceframe.NewIncorrectInputLengthError(len(to), len(wf.DoF())) + return nil, referenceframe.NewIncorrectDoFError(len(to), len(wf.DoF())) } interp := make([]referenceframe.Input, 0, len(to)) @@ -97,7 +97,7 @@ func (wf *wrapperFrame) Interpolate(from, to []referenceframe.Input, by float64) // Geometries returns an object representing the 3D space associated with the executionFrame's geometry. func (wf *wrapperFrame) Geometries(inputs []referenceframe.Input) (*referenceframe.GeometriesInFrame, error) { if len(inputs) != len(wf.DoF()) { - return nil, referenceframe.NewIncorrectInputLengthError(len(inputs), len(wf.DoF())) + return nil, referenceframe.NewIncorrectDoFError(len(inputs), len(wf.DoF())) } sfGeometries := []spatialmath.Geometry{} gf, err := wf.executionFrame.Geometries(make([]referenceframe.Input, len(wf.executionFrame.DoF()))) diff --git a/services/motion/motion_helpers_test.go b/services/motion/motion_helpers_test.go index 694c1f07380..f1274b64ddd 100644 --- a/services/motion/motion_helpers_test.go +++ b/services/motion/motion_helpers_test.go @@ -7,7 +7,6 @@ import ( "github.com/golang/geo/r3" "github.com/pkg/errors" - pb "go.viam.com/api/component/arm/v1" "go.viam.com/test" "go.viam.com/rdk/components/arm" @@ -117,8 +116,8 @@ func TestOOBArmMotion(t *testing.T) { OOBFloats := []float64{0, 0, 0, 0, 0, 720} injectedArm := &inject.Arm{ Arm: notReal, - JointPositionsFunc: func(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { - return &pb.JointPositions{Values: OOBFloats}, nil + JointPositionsFunc: func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { + return referenceframe.FloatsToInputs(OOBFloats), nil }, CurrentInputsFunc: func(ctx context.Context) ([]referenceframe.Input, error) { return referenceframe.FloatsToInputs(OOBFloats), nil @@ -139,12 +138,12 @@ func TestOOBArmMotion(t *testing.T) { }) t.Run("MoveToJointPositions fails OOB and moving further OOB", func(t *testing.T) { - err := injectedArm.MoveToJointPositions(context.Background(), &pb.JointPositions{Values: []float64{0, 0, 0, 0, 0, 900}}, nil) + err := injectedArm.MoveToJointPositions(context.Background(), referenceframe.FloatsToInputs([]float64{0, 0, 0, 0, 0, 900}), nil) test.That(t, err, test.ShouldNotBeNil) }) t.Run("MoveToJointPositions succeeds when OOB and moving further in-bounds", func(t *testing.T) { - err := injectedArm.MoveToJointPositions(context.Background(), &pb.JointPositions{Values: []float64{0, 0, 0, 0, 0, 0}}, nil) + err := injectedArm.MoveToJointPositions(context.Background(), referenceframe.FloatsToInputs([]float64{0, 0, 0, 0, 0, 0}), nil) test.That(t, err, test.ShouldBeNil) }) } diff --git a/testutils/inject/arm.go b/testutils/inject/arm.go index 8079a08b914..df0d92bba3c 100644 --- a/testutils/inject/arm.go +++ b/testutils/inject/arm.go @@ -3,8 +3,6 @@ package inject import ( "context" - pb "go.viam.com/api/component/arm/v1" - "go.viam.com/rdk/components/arm" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" @@ -18,8 +16,8 @@ type Arm struct { DoFunc func(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) EndPositionFunc func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) MoveToPositionFunc func(ctx context.Context, to spatialmath.Pose, extra map[string]interface{}) error - MoveToJointPositionsFunc func(ctx context.Context, pos *pb.JointPositions, extra map[string]interface{}) error - JointPositionsFunc func(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) + MoveToJointPositionsFunc func(ctx context.Context, positions []referenceframe.Input, extra map[string]interface{}) error + JointPositionsFunc func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) StopFunc func(ctx context.Context, extra map[string]interface{}) error IsMovingFunc func(context.Context) (bool, error) CloseFunc func(ctx context.Context) error @@ -56,15 +54,15 @@ func (a *Arm) MoveToPosition(ctx context.Context, to spatialmath.Pose, extra map } // MoveToJointPositions calls the injected MoveToJointPositions or the real version. -func (a *Arm) MoveToJointPositions(ctx context.Context, jp *pb.JointPositions, extra map[string]interface{}) error { +func (a *Arm) MoveToJointPositions(ctx context.Context, positions []referenceframe.Input, extra map[string]interface{}) error { if a.MoveToJointPositionsFunc == nil { - return a.Arm.MoveToJointPositions(ctx, jp, extra) + return a.Arm.MoveToJointPositions(ctx, positions, extra) } - return a.MoveToJointPositionsFunc(ctx, jp, extra) + return a.MoveToJointPositionsFunc(ctx, positions, extra) } // JointPositions calls the injected JointPositions or the real version. -func (a *Arm) JointPositions(ctx context.Context, extra map[string]interface{}) (*pb.JointPositions, error) { +func (a *Arm) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { if a.JointPositionsFunc == nil { return a.Arm.JointPositions(ctx, extra) } From 8116d33868ee28335cee0ce737ebab290a876e68 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Tue, 29 Oct 2024 14:41:09 -0400 Subject: [PATCH 03/14] lint --- components/arm/arm.go | 3 --- components/arm/eva/eva.go | 1 - components/arm/universalrobots/ur.go | 1 - go.sum | 2 -- referenceframe/input.go | 4 ++++ robot/impl/local_robot_test.go | 1 - 6 files changed, 4 insertions(+), 8 deletions(-) diff --git a/components/arm/arm.go b/components/arm/arm.go index 2f86aca4871..369d71e65bd 100644 --- a/components/arm/arm.go +++ b/components/arm/arm.go @@ -8,7 +8,6 @@ package arm import ( "context" - "errors" "fmt" v1 "go.viam.com/api/common/v1" @@ -23,8 +22,6 @@ import ( "go.viam.com/rdk/utils" ) -var ErrModelNotValid = errors.New("unable to retrieve a valid arm model from arm") - func init() { resource.RegisterAPI(API, resource.APIRegistration[Arm]{ Status: resource.StatusFunc(CreateStatus), diff --git a/components/arm/eva/eva.go b/components/arm/eva/eva.go index b32d0a6ca10..b7d93204667 100644 --- a/components/arm/eva/eva.go +++ b/components/arm/eva/eva.go @@ -6,7 +6,6 @@ package eva import ( "bytes" "context" - // for embedding model file. _ "embed" "encoding/json" diff --git a/components/arm/universalrobots/ur.go b/components/arm/universalrobots/ur.go index e81eaae6d7b..49889fd7e40 100644 --- a/components/arm/universalrobots/ur.go +++ b/components/arm/universalrobots/ur.go @@ -4,7 +4,6 @@ package universalrobots import ( "bufio" "context" - // for embedding model file. _ "embed" "encoding/binary" diff --git a/go.sum b/go.sum index e244c7d0e89..d39f0257674 100644 --- a/go.sum +++ b/go.sum @@ -1645,8 +1645,6 @@ go.uber.org/zap v1.18.1/go.mod h1:xg/QME4nWcxGxrpdeYfq7UvYrLh66cuVKdrbD1XF/NI= go.uber.org/zap v1.23.0/go.mod h1:D+nX8jyLsMHMYrln8A0rJjFt/T/9/bGgIhAqxv5URuY= go.uber.org/zap v1.27.0 h1:aJMhYGrd5QSmlpLMr2MftRKl7t8J8PTZPA732ud/XR8= go.uber.org/zap v1.27.0/go.mod h1:GB2qFLM7cTU87MWRP2mPIjqfIDnGu+VIO4V/SdhGo2E= -go.viam.com/api v0.1.350 h1:pMyU/fFS4cEsQNQoC69lBapwl6R2jbN2q4hoT+Sz8lw= -go.viam.com/api v0.1.350/go.mod h1:5lpVRxMsKFCaahqsnJfPGwJ9baoQ6PIKQu3lxvy6Wtw= go.viam.com/api v0.1.351 h1:jNIpa7K1RzQzWrwnXJBwFZ+oQ/OiZBWLqTp9+7nznPI= go.viam.com/api v0.1.351/go.mod h1:5lpVRxMsKFCaahqsnJfPGwJ9baoQ6PIKQu3lxvy6Wtw= go.viam.com/test v1.1.1-0.20220913152726-5da9916c08a2 h1:oBiK580EnEIzgFLU4lHOXmGAE3MxnVbeR7s1wp/F3Ps= diff --git a/referenceframe/input.go b/referenceframe/input.go index 467532be170..e7e7c37877a 100644 --- a/referenceframe/input.go +++ b/referenceframe/input.go @@ -17,6 +17,8 @@ type Input struct { Value float64 } +// JointPositionsFromInputs converts the given slice of Input to a JointPositions struct, +// using the ProtobufFromInput function provided by the given Frame. func JointPositionsFromInputs(f Frame, inputs []Input) (*pb.JointPositions, error) { if f == nil { // if a frame is not provided, we will assume all inputs are specified in degrees and need to be converted to radians @@ -28,6 +30,8 @@ func JointPositionsFromInputs(f Frame, inputs []Input) (*pb.JointPositions, erro return f.ProtobufFromInput(inputs), nil } +// InputsFromJointPositions converts the given JointPositions struct to a slice of Input, +// using the ProtobufFromInput function provided by the given Frame. func InputsFromJointPositions(f Frame, jp *pb.JointPositions) ([]Input, error) { if f == nil { // if a frame is not provided, we will assume all inputs are specified in degrees and need to be converted to radians diff --git a/robot/impl/local_robot_test.go b/robot/impl/local_robot_test.go index e9743f5317a..f1fcb5339ce 100644 --- a/robot/impl/local_robot_test.go +++ b/robot/impl/local_robot_test.go @@ -20,7 +20,6 @@ import ( "github.com/golang/geo/r3" "go.mongodb.org/mongo-driver/bson/primitive" "go.uber.org/zap" - // registers all components. commonpb "go.viam.com/api/common/v1" armpb "go.viam.com/api/component/arm/v1" From 3e83af9e5946b9422662ae8ee98cc0eaea8caefe Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Tue, 29 Oct 2024 15:45:36 -0400 Subject: [PATCH 04/14] lint --- motionplan/motionPlanner_test.go | 4 ++-- services/datamanager/builtin/builtin_test.go | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/motionplan/motionPlanner_test.go b/motionplan/motionPlanner_test.go index 0a710e86339..0b1c8fda434 100644 --- a/motionplan/motionPlanner_test.go +++ b/motionplan/motionPlanner_test.go @@ -1148,7 +1148,7 @@ func TestValidatePlanRequest(t *testing.T) { "frame1": frame.FloatsToInputs([]float64{0}), }, }, - expectedErr: errors.New("number of inputs does not match frame DoF, expected 0 but got 1"), + expectedErr: frame.NewIncorrectDoFError(1, 0), }, { name: "incorrect length StartConfiguration - fail", @@ -1171,7 +1171,7 @@ func TestValidatePlanRequest(t *testing.T) { "frame2": frame.FloatsToInputs([]float64{0, 0, 0, 0, 0}), }, }, - expectedErr: errors.New("number of inputs does not match frame DoF, expected 1 but got 5"), + expectedErr: frame.NewIncorrectDoFError(5, 1), }, { name: "well formed PlanRequest", diff --git a/services/datamanager/builtin/builtin_test.go b/services/datamanager/builtin/builtin_test.go index 9a0de0b5adb..274f0da8ea4 100644 --- a/services/datamanager/builtin/builtin_test.go +++ b/services/datamanager/builtin/builtin_test.go @@ -287,6 +287,9 @@ func TestFileDeletion(t *testing.T) { ) ([]referenceframe.Input, error) { return referenceframe.FloatsToInputs([]float64{1.0, 2.0, 3.0, 4.0}), nil }, + ModelFrameFunc: func() referenceframe.Model { + return nil + }, }, gantry.Named("gantry1"): &inject.Gantry{ PositionFunc: func( From 3ededbcf674456181f8fc1d0940b0a636ec9e42c Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Wed, 30 Oct 2024 11:42:03 -0400 Subject: [PATCH 05/14] lint --- components/arm/client.go | 1 + referenceframe/input.go | 7 +++++++ 2 files changed, 8 insertions(+) diff --git a/components/arm/client.go b/components/arm/client.go index 42d0ad3c597..6abb04e7abd 100644 --- a/components/arm/client.go +++ b/components/arm/client.go @@ -145,6 +145,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 diff --git a/referenceframe/input.go b/referenceframe/input.go index e7e7c37877a..1bcd1132e13 100644 --- a/referenceframe/input.go +++ b/referenceframe/input.go @@ -1,6 +1,7 @@ package referenceframe import ( + "errors" "fmt" "math" @@ -24,6 +25,9 @@ func JointPositionsFromInputs(f Frame, inputs []Input) (*pb.JointPositions, erro // if a frame is not provided, we will assume all inputs are specified in degrees and need to be converted to radians return JointPositionsFromRadians(InputsToFloats(inputs)), nil } + if inputs == nil { + return nil, errors.New("inputs cannot be nil") + } if len(f.DoF()) != len(inputs) { return nil, NewIncorrectDoFError(len(inputs), len(f.DoF())) } @@ -37,6 +41,9 @@ func InputsFromJointPositions(f Frame, jp *pb.JointPositions) ([]Input, error) { // if a frame is not provided, we will assume all inputs are specified in degrees and need to be converted to radians return FloatsToInputs(JointPositionsToRadians(jp)), nil } + if jp == nil { + return nil, errors.New("jointPositions cannot be nil") + } if len(f.DoF()) != len(jp.Values) { return nil, NewIncorrectDoFError(len(jp.Values), len(f.DoF())) } From d18ff0c26a4bea0458fbf03e6c2cb1fc6c6b0fed Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Wed, 30 Oct 2024 12:21:18 -0400 Subject: [PATCH 06/14] lint --- referenceframe/input.go | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/referenceframe/input.go b/referenceframe/input.go index 1bcd1132e13..9ce24f4c880 100644 --- a/referenceframe/input.go +++ b/referenceframe/input.go @@ -25,12 +25,12 @@ func JointPositionsFromInputs(f Frame, inputs []Input) (*pb.JointPositions, erro // if a frame is not provided, we will assume all inputs are specified in degrees and need to be converted to radians return JointPositionsFromRadians(InputsToFloats(inputs)), nil } - if inputs == nil { - return nil, errors.New("inputs cannot be nil") - } if len(f.DoF()) != len(inputs) { return nil, NewIncorrectDoFError(len(inputs), len(f.DoF())) } + if inputs == nil { + return nil, errors.New("inputs cannot be nil") + } return f.ProtobufFromInput(inputs), nil } @@ -41,12 +41,12 @@ func InputsFromJointPositions(f Frame, jp *pb.JointPositions) ([]Input, error) { // if a frame is not provided, we will assume all inputs are specified in degrees and need to be converted to radians return FloatsToInputs(JointPositionsToRadians(jp)), nil } - if jp == nil { - return nil, errors.New("jointPositions cannot be nil") - } if len(f.DoF()) != len(jp.Values) { return nil, NewIncorrectDoFError(len(jp.Values), len(f.DoF())) } + if jp == nil { + return nil, errors.New("jointPositions cannot be nil") + } return f.InputFromProtobuf(jp), nil } From 5cfecfa8951d7826cc6da574791e1f710761b6b2 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Wed, 30 Oct 2024 15:53:34 -0400 Subject: [PATCH 07/14] first pass --- components/arm/arm.go | 29 +++++++++++++------ components/arm/client.go | 33 ++++++++++++++++++++-- components/arm/eva/eva.go | 10 +++++++ components/arm/fake/fake.go | 9 ++++++ components/arm/pb_helpers.go | 42 ++++++++++++++++++++++++++++ components/arm/server.go | 22 +++++++++++++++ components/arm/universalrobots/ur.go | 10 +++++++ components/arm/wrapper/wrapper.go | 9 ++++++ components/arm/xarm/xarm_comm.go | 10 +++++++ 9 files changed, 163 insertions(+), 11 deletions(-) diff --git a/components/arm/arm.go b/components/arm/arm.go index 369d71e65bd..99754ba7ee5 100644 --- a/components/arm/arm.go +++ b/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: // @@ -109,10 +118,14 @@ type Arm interface { // This will block until done or a new operation cancels this one. MoveToPosition(ctx context.Context, pose spatialmath.Pose, extra map[string]interface{}) error - // MoveToJointPositions moves the arm's joints to the given positions. + // MoveToJointPositions 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. MoveToJointPositions(ctx context.Context, positions []referenceframe.Input, extra map[string]interface{}) error + // MoveToJointPositions moves the arm's joints to the given positions. + // 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) } diff --git a/components/arm/client.go b/components/arm/client.go index 6abb04e7abd..d872e60a846 100644 --- a/components/arm/client.go +++ b/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,36 @@ 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) + } + _, err = c.client.MoveThroughJointPositions(ctx, &pb.MoveThroughJointPositionsRequest{ + Name: c.name, + Positions: allJPs, + Options: options.toProtobuf(), + Extra: ext, + }) + return err +} + func (c *client) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { ext, err := protoutils.StructToStructPb(extra) if err != nil { diff --git a/components/arm/eva/eva.go b/components/arm/eva/eva.go index b7d93204667..28f45fbc524 100644 --- a/components/arm/eva/eva.go +++ b/components/arm/eva/eva.go @@ -6,6 +6,7 @@ package eva import ( "bytes" "context" + // for embedding model file. _ "embed" "encoding/json" @@ -177,6 +178,15 @@ 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 { + return e.GoToInputs(ctx, positions...) +} + func (e *eva) doMoveJoints(ctx context.Context, joints []float64) error { if err := e.apiLock(ctx); err != nil { return err diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index 4f9a4433c90..3fecb38beff 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -188,6 +188,15 @@ func (a *Arm) MoveToJointPositions(ctx context.Context, joints []referenceframe. return nil } +func (a *Arm) MoveThroughJointPositions( + ctx context.Context, + positions [][]referenceframe.Input, + _ *arm.MoveOptions, + _ map[string]interface{}, +) error { + return a.GoToInputs(ctx, positions...) +} + // JointPositions returns joints. func (a *Arm) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { a.mu.RLock() diff --git a/components/arm/pb_helpers.go b/components/arm/pb_helpers.go index 2c8da8b5127..1e533b07168 100644 --- a/components/arm/pb_helpers.go +++ b/components/arm/pb_helpers.go @@ -4,11 +4,53 @@ 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" ) +const ( + defaultMaxVelDegPerSec = 60. + defaultMaxAccDegPerSec2 = 1. +) + +type MoveOptions struct { + MaxVel, MaxAcc float64 +} + +func NewDefaultMoveOptions() *MoveOptions { + return moveOptionsFromProtobuf(nil) +} + +func moveOptionsFromProtobuf(protobuf *pb.MoveOptions) *MoveOptions { + if protobuf == nil { + protobuf = &pb.MoveOptions{} + } + + var vel, acc float64 + if protobuf.MaxVelDegsPerSec == nil { + vel = defaultMaxVelDegPerSec + } + if protobuf.MaxAccDegsPerSec2 == nil { + acc = defaultMaxAccDegPerSec2 + } + return &MoveOptions{ + MaxVel: utils.DegToRad(vel), + MaxAcc: utils.DegToRad(acc), + } +} + +func (opts MoveOptions) toProtobuf() *pb.MoveOptions { + vel := utils.RadToDeg(opts.MaxVel) + acc := utils.RadToDeg(opts.MaxAcc) + return &pb.MoveOptions{ + MaxVelDegsPerSec: &vel, + MaxAccDegsPerSec2: &acc, + } +} + func parseKinematicsResponse(name string, resp *commonpb.GetKinematicsResponse) (referenceframe.Model, error) { format := resp.GetFormat() data := resp.GetKinematicsData() diff --git a/components/arm/server.go b/components/arm/server.go index 72d65042f07..cc6d7c544c5 100644 --- a/components/arm/server.go +++ b/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.MoveToJointPositionsResponse, 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.MoveToJointPositionsResponse{}, err +} + // Stop stops the arm specified. func (s *serviceServer) Stop(ctx context.Context, req *pb.StopRequest) (*pb.StopResponse, error) { operation.CancelOtherWithLabel(ctx, req.Name) diff --git a/components/arm/universalrobots/ur.go b/components/arm/universalrobots/ur.go index 49889fd7e40..9b384791068 100644 --- a/components/arm/universalrobots/ur.go +++ b/components/arm/universalrobots/ur.go @@ -4,6 +4,7 @@ package universalrobots import ( "bufio" "context" + // for embedding model file. _ "embed" "encoding/binary" @@ -383,6 +384,15 @@ func (ua *urArm) MoveToJointPositions(ctx context.Context, joints []referencefra return ua.moveToJointPositionRadians(ctx, referenceframe.InputsToFloats(joints)) } +func (ua *urArm) MoveThroughJointPositions( + ctx context.Context, + positions [][]referenceframe.Input, + _ *arm.MoveOptions, + _ map[string]interface{}, +) error { + return ua.GoToInputs(ctx, positions...) +} + // Stop stops the arm with some deceleration. func (ua *urArm) Stop(ctx context.Context, extra map[string]interface{}) error { if !ua.inRemoteMode { diff --git a/components/arm/wrapper/wrapper.go b/components/arm/wrapper/wrapper.go index ca4d7815c80..33f78af1c1a 100644 --- a/components/arm/wrapper/wrapper.go +++ b/components/arm/wrapper/wrapper.go @@ -135,6 +135,15 @@ func (wrapper *Arm) MoveToJointPositions(ctx context.Context, joints []reference return wrapper.actual.MoveToJointPositions(ctx, joints, extra) } +func (wrapper *Arm) MoveThroughJointPositions( + ctx context.Context, + positions [][]referenceframe.Input, + _ *arm.MoveOptions, + _ map[string]interface{}, +) error { + return wrapper.GoToInputs(ctx, positions...) +} + // JointPositions returns the set joints. func (wrapper *Arm) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { wrapper.mu.RLock() diff --git a/components/arm/xarm/xarm_comm.go b/components/arm/xarm/xarm_comm.go index a2592d294a3..45ac4f4702c 100644 --- a/components/arm/xarm/xarm_comm.go +++ b/components/arm/xarm/xarm_comm.go @@ -10,6 +10,7 @@ import ( "go.uber.org/multierr" "go.viam.com/utils" + "go.viam.com/rdk/components/arm" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/services/motion" "go.viam.com/rdk/spatialmath" @@ -358,6 +359,15 @@ func (x *xArm) MoveToJointPositions(ctx context.Context, newPositions []referenc return x.GoToInputs(ctx, newPositions) } +func (x *xArm) MoveThroughJointPositions( + ctx context.Context, + positions [][]referenceframe.Input, + _ *arm.MoveOptions, + _ map[string]interface{}, +) error { + return x.GoToInputs(ctx, positions...) +} + // Using the configured moveHz, joint speed, and joint acceleration, create the series of joint positions for the arm to follow, // using a trapezoidal velocity profile to blend between waypoints to the extent possible. func (x *xArm) createRawJointSteps(startInputs []referenceframe.Input, inputSteps [][]referenceframe.Input) ([][]float64, error) { From c5933714ee263713281b8d89a6919896353cddd4 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Wed, 30 Oct 2024 16:02:01 -0400 Subject: [PATCH 08/14] fix comment --- components/arm/arm.go | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/components/arm/arm.go b/components/arm/arm.go index 99754ba7ee5..2b18e3b3106 100644 --- a/components/arm/arm.go +++ b/components/arm/arm.go @@ -118,11 +118,11 @@ type Arm interface { // This will block until done or a new operation cancels this one. MoveToPosition(ctx context.Context, pose spatialmath.Pose, extra map[string]interface{}) error - // MoveToJointPositions moves the arm's joints through the given positions in the order they are specified. + // MoveToJointPositions moves the arm's joints to the given positions. // This will block until done or a new operation cancels this one. MoveToJointPositions(ctx context.Context, positions []referenceframe.Input, extra map[string]interface{}) error - // MoveToJointPositions moves the arm's joints to the given positions. + // 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 From b799883eda9aff17a59bd5b735e9000f82454e37 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Wed, 30 Oct 2024 16:33:49 -0400 Subject: [PATCH 09/14] invert who calls who --- components/arm/arm.go | 16 ---------------- components/arm/eva/eva.go | 22 +++++++++++----------- components/arm/fake/fake.go | 15 ++++++++------- components/arm/universalrobots/ur.go | 24 ++++++++++++------------ components/arm/wrapper/wrapper.go | 26 ++++++++++++-------------- components/arm/xarm/xarm.go | 16 +--------------- components/arm/xarm/xarm_comm.go | 16 +++++++++++++++- services/motion/motion_helpers.go | 2 +- 8 files changed, 60 insertions(+), 77 deletions(-) diff --git a/components/arm/arm.go b/components/arm/arm.go index 2b18e3b3106..67b291fbd81 100644 --- a/components/arm/arm.go +++ b/components/arm/arm.go @@ -172,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 { diff --git a/components/arm/eva/eva.go b/components/arm/eva/eva.go index 28f45fbc524..c6f5aeaf9a5 100644 --- a/components/arm/eva/eva.go +++ b/components/arm/eva/eva.go @@ -184,7 +184,16 @@ func (e *eva) MoveThroughJointPositions( _ *arm.MoveOptions, _ map[string]interface{}, ) error { - return e.GoToInputs(ctx, positions...) + 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 { @@ -394,16 +403,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, arm.NewDefaultMoveOptions(), nil) } func (e *eva) Close(ctx context.Context) error { diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index 3fecb38beff..966eb70ebdb 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -188,13 +188,19 @@ func (a *Arm) MoveToJointPositions(ctx context.Context, joints []referenceframe. return nil } +// GoToInputs moves the fake arm through the given inputs. func (a *Arm) MoveThroughJointPositions( ctx context.Context, positions [][]referenceframe.Input, _ *arm.MoveOptions, _ map[string]interface{}, ) error { - return a.GoToInputs(ctx, positions...) + for _, goal := range positions { + if err := a.MoveToJointPositions(ctx, goal, nil); err != nil { + return err + } + } + return nil } // JointPositions returns joints. @@ -223,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, arm.NewDefaultMoveOptions(), nil) } // Close does nothing. diff --git a/components/arm/universalrobots/ur.go b/components/arm/universalrobots/ur.go index 9b384791068..2c6f2677b68 100644 --- a/components/arm/universalrobots/ur.go +++ b/components/arm/universalrobots/ur.go @@ -390,7 +390,17 @@ func (ua *urArm) MoveThroughJointPositions( _ *arm.MoveOptions, _ map[string]interface{}, ) error { - return ua.GoToInputs(ctx, positions...) + for _, goal := range positions { + // check that joint positions are not out of bounds + if err := arm.CheckDesiredJointPositions(ctx, ua, goal); err != nil { + return err + } + err := ua.MoveToJointPositions(ctx, goal, nil) + if err != nil { + return err + } + } + return nil } // Stop stops the arm with some deceleration. @@ -507,17 +517,7 @@ func (ua *urArm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, err // GoToInputs moves the UR arm to the Inputs specified. func (ua *urArm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { - for _, goal := range inputSteps { - // check that joint positions are not out of bounds - if err := arm.CheckDesiredJointPositions(ctx, ua, goal); err != nil { - return err - } - err := ua.MoveToJointPositions(ctx, goal, nil) - if err != nil { - return err - } - } - return nil + return ua.MoveThroughJointPositions(ctx, inputSteps, arm.NewDefaultMoveOptions(), nil) } // Geometries returns the list of geometries associated with the resource, in any order. The poses of the geometries reflect their diff --git a/components/arm/wrapper/wrapper.go b/components/arm/wrapper/wrapper.go index 33f78af1c1a..db8bf9b1f4e 100644 --- a/components/arm/wrapper/wrapper.go +++ b/components/arm/wrapper/wrapper.go @@ -141,7 +141,17 @@ func (wrapper *Arm) MoveThroughJointPositions( _ *arm.MoveOptions, _ map[string]interface{}, ) error { - return wrapper.GoToInputs(ctx, positions...) + for _, goal := range positions { + // check that joint positions are not out of bounds + if err := arm.CheckDesiredJointPositions(ctx, wrapper, goal); err != nil { + return err + } + err := wrapper.MoveToJointPositions(ctx, goal, nil) + if err != nil { + return err + } + } + return nil } // JointPositions returns the set joints. @@ -173,24 +183,12 @@ func (wrapper *Arm) IsMoving(ctx context.Context) (bool, error) { // CurrentInputs returns the current inputs of the arm. func (wrapper *Arm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { - wrapper.mu.RLock() - defer wrapper.mu.RUnlock() return wrapper.actual.JointPositions(ctx, nil) } // GoToInputs moves the arm to the specified goal inputs. func (wrapper *Arm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { - for _, goal := range inputSteps { - // check that joint positions are not out of bounds - if err := arm.CheckDesiredJointPositions(ctx, wrapper, goal); err != nil { - return err - } - err := wrapper.MoveToJointPositions(ctx, goal, nil) - if err != nil { - return err - } - } - return nil + return wrapper.MoveThroughJointPositions(ctx, inputSteps, arm.NewDefaultMoveOptions(), nil) } // Geometries returns the list of geometries associated with the resource, in any order. The poses of the geometries reflect their diff --git a/components/arm/xarm/xarm.go b/components/arm/xarm/xarm.go index 01388beed57..500d54bd369 100644 --- a/components/arm/xarm/xarm.go +++ b/components/arm/xarm/xarm.go @@ -195,21 +195,7 @@ func (x *xArm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error } func (x *xArm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { - for _, goal := range inputSteps { - // check that joint positions are not out of bounds - if err := arm.CheckDesiredJointPositions(ctx, x, goal); err != nil { - return err - } - } - curPos, err := x.JointPositions(ctx, nil) - if err != nil { - return err - } - armRawSteps, err := x.createRawJointSteps(curPos, inputSteps) - if err != nil { - return err - } - return x.executeInputs(ctx, armRawSteps) + return x.MoveThroughJointPositions(ctx, inputSteps, arm.NewDefaultMoveOptions(), nil) } func (x *xArm) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { diff --git a/components/arm/xarm/xarm_comm.go b/components/arm/xarm/xarm_comm.go index 45ac4f4702c..50e5396817b 100644 --- a/components/arm/xarm/xarm_comm.go +++ b/components/arm/xarm/xarm_comm.go @@ -365,7 +365,21 @@ func (x *xArm) MoveThroughJointPositions( _ *arm.MoveOptions, _ map[string]interface{}, ) error { - return x.GoToInputs(ctx, positions...) + for _, goal := range positions { + // check that joint positions are not out of bounds + if err := arm.CheckDesiredJointPositions(ctx, x, goal); err != nil { + return err + } + } + curPos, err := x.JointPositions(ctx, nil) + if err != nil { + return err + } + armRawSteps, err := x.createRawJointSteps(curPos, positions) + if err != nil { + return err + } + return x.executeInputs(ctx, armRawSteps) } // Using the configured moveHz, joint speed, and joint acceleration, create the series of joint positions for the arm to follow, diff --git a/services/motion/motion_helpers.go b/services/motion/motion_helpers.go index 0d0149091a3..a16f4e72542 100644 --- a/services/motion/motion_helpers.go +++ b/services/motion/motion_helpers.go @@ -84,5 +84,5 @@ func MoveArm(ctx context.Context, logger logging.Logger, a arm.Arm, dst spatialm if err != nil { return err } - return arm.GoToWaypoints(ctx, a, plan) + return a.MoveThroughJointPositions(ctx, plan, arm.NewDefaultMoveOptions(), nil) } From 595efa3f9844cca840da97492d0a6c4f36a54f24 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 31 Oct 2024 16:26:15 -0400 Subject: [PATCH 10/14] remove moveOptions defaults --- components/arm/eva/eva.go | 2 +- components/arm/fake/fake.go | 2 +- components/arm/pb_helpers.go | 13 ++----------- components/arm/universalrobots/ur.go | 2 +- components/arm/wrapper/wrapper.go | 2 +- components/arm/xarm/xarm.go | 2 +- 6 files changed, 7 insertions(+), 16 deletions(-) diff --git a/components/arm/eva/eva.go b/components/arm/eva/eva.go index c6f5aeaf9a5..43da89af3f0 100644 --- a/components/arm/eva/eva.go +++ b/components/arm/eva/eva.go @@ -403,7 +403,7 @@ func (e *eva) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) } func (e *eva) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { - return e.MoveThroughJointPositions(ctx, inputSteps, arm.NewDefaultMoveOptions(), nil) + return e.MoveThroughJointPositions(ctx, inputSteps, nil, nil) } func (e *eva) Close(ctx context.Context) error { diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index 966eb70ebdb..e9ede7f28d1 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -229,7 +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 { - return a.MoveThroughJointPositions(ctx, inputSteps, arm.NewDefaultMoveOptions(), nil) + return a.MoveThroughJointPositions(ctx, inputSteps, nil, nil) } // Close does nothing. diff --git a/components/arm/pb_helpers.go b/components/arm/pb_helpers.go index 1e533b07168..07a778c1233 100644 --- a/components/arm/pb_helpers.go +++ b/components/arm/pb_helpers.go @@ -11,19 +11,10 @@ import ( "go.viam.com/rdk/utils" ) -const ( - defaultMaxVelDegPerSec = 60. - defaultMaxAccDegPerSec2 = 1. -) - type MoveOptions struct { MaxVel, MaxAcc float64 } -func NewDefaultMoveOptions() *MoveOptions { - return moveOptionsFromProtobuf(nil) -} - func moveOptionsFromProtobuf(protobuf *pb.MoveOptions) *MoveOptions { if protobuf == nil { protobuf = &pb.MoveOptions{} @@ -31,10 +22,10 @@ func moveOptionsFromProtobuf(protobuf *pb.MoveOptions) *MoveOptions { var vel, acc float64 if protobuf.MaxVelDegsPerSec == nil { - vel = defaultMaxVelDegPerSec + vel = 0 } if protobuf.MaxAccDegsPerSec2 == nil { - acc = defaultMaxAccDegPerSec2 + acc = 0 } return &MoveOptions{ MaxVel: utils.DegToRad(vel), diff --git a/components/arm/universalrobots/ur.go b/components/arm/universalrobots/ur.go index 2c6f2677b68..aa922759001 100644 --- a/components/arm/universalrobots/ur.go +++ b/components/arm/universalrobots/ur.go @@ -517,7 +517,7 @@ func (ua *urArm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, err // GoToInputs moves the UR arm to the Inputs specified. func (ua *urArm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { - return ua.MoveThroughJointPositions(ctx, inputSteps, arm.NewDefaultMoveOptions(), nil) + return ua.MoveThroughJointPositions(ctx, inputSteps, nil, nil) } // Geometries returns the list of geometries associated with the resource, in any order. The poses of the geometries reflect their diff --git a/components/arm/wrapper/wrapper.go b/components/arm/wrapper/wrapper.go index db8bf9b1f4e..e6ab33a6947 100644 --- a/components/arm/wrapper/wrapper.go +++ b/components/arm/wrapper/wrapper.go @@ -188,7 +188,7 @@ func (wrapper *Arm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, // GoToInputs moves the arm to the specified goal inputs. func (wrapper *Arm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { - return wrapper.MoveThroughJointPositions(ctx, inputSteps, arm.NewDefaultMoveOptions(), nil) + return wrapper.MoveThroughJointPositions(ctx, inputSteps, nil, nil) } // Geometries returns the list of geometries associated with the resource, in any order. The poses of the geometries reflect their diff --git a/components/arm/xarm/xarm.go b/components/arm/xarm/xarm.go index 500d54bd369..cf7e050f585 100644 --- a/components/arm/xarm/xarm.go +++ b/components/arm/xarm/xarm.go @@ -195,7 +195,7 @@ func (x *xArm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error } func (x *xArm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { - return x.MoveThroughJointPositions(ctx, inputSteps, arm.NewDefaultMoveOptions(), nil) + return x.MoveThroughJointPositions(ctx, inputSteps, nil, nil) } func (x *xArm) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { From 9d3967e9e66c1ad01379c2c0164d452a20496eab Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 31 Oct 2024 17:24:10 -0400 Subject: [PATCH 11/14] add tests --- components/arm/client_test.go | 35 +++++++++++++++++++----- components/arm/pb_helpers.go | 8 +++--- components/arm/server.go | 4 +-- components/arm/server_test.go | 32 +++++++++++++++++++++- services/motion/motion_helpers.go | 2 +- testutils/inject/arm.go | 45 ++++++++++++++++++++++--------- 6 files changed, 98 insertions(+), 28 deletions(-) diff --git a/components/arm/client_test.go b/components/arm/client_test.go index a2e50585b54..9381e7ec9f1 100644 --- a/components/arm/client_test.go +++ b/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{MaxVel: 1, MaxAcc: 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()) diff --git a/components/arm/pb_helpers.go b/components/arm/pb_helpers.go index 07a778c1233..70e2cf12d1d 100644 --- a/components/arm/pb_helpers.go +++ b/components/arm/pb_helpers.go @@ -21,11 +21,11 @@ func moveOptionsFromProtobuf(protobuf *pb.MoveOptions) *MoveOptions { } var vel, acc float64 - if protobuf.MaxVelDegsPerSec == nil { - vel = 0 + if protobuf.MaxVelDegsPerSec != nil { + vel = *protobuf.MaxVelDegsPerSec } - if protobuf.MaxAccDegsPerSec2 == nil { - acc = 0 + if protobuf.MaxAccDegsPerSec2 != nil { + acc = *protobuf.MaxAccDegsPerSec2 } return &MoveOptions{ MaxVel: utils.DegToRad(vel), diff --git a/components/arm/server.go b/components/arm/server.go index cc6d7c544c5..65abeef1c51 100644 --- a/components/arm/server.go +++ b/components/arm/server.go @@ -107,7 +107,7 @@ func (s *serviceServer) MoveToJointPositions( func (s *serviceServer) MoveThroughJointPositions( ctx context.Context, req *pb.MoveThroughJointPositionsRequest, -) (*pb.MoveToJointPositionsResponse, error) { +) (*pb.MoveThroughJointPositionsResponse, error) { operation.CancelOtherWithLabel(ctx, req.Name) arm, err := s.coll.Resource(req.Name) if err != nil { @@ -122,7 +122,7 @@ func (s *serviceServer) MoveThroughJointPositions( allInputs = append(allInputs, inputs) } err = arm.MoveThroughJointPositions(ctx, allInputs, moveOptionsFromProtobuf(req.Options), req.Extra.AsMap()) - return &pb.MoveToJointPositionsResponse{}, err + return &pb.MoveThroughJointPositionsResponse{}, err } // Stop stops the arm specified. diff --git a/components/arm/server_test.go b/components/arm/server_test.go index daca3d95a1f..1fa3bb1ab42 100644 --- a/components/arm/server_test.go +++ b/components/arm/server_test.go @@ -50,6 +50,7 @@ func TestServer(t *testing.T) { var ( capArmPos spatialmath.Pose capArmJointPos []referenceframe.Input + moveOptions arm.MoveOptions extraOptions map[string]interface{} ) @@ -68,12 +69,22 @@ func TestServer(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.ModelFrameFunc = func() referenceframe.Model { model, err := urdf.ParseModelXMLFile(utils.ResolveFile("referenceframe/urdf/testfiles/ur5e.urdf"), "foo") if err != nil { @@ -210,6 +221,25 @@ func TestServer(t *testing.T) { test.That(t, err.Error(), test.ShouldContainSubstring, errMoveToJointPositionFailed.Error()) }) + t.Run("move through joint positions", func(t *testing.T) { + ext, err := protoutils.StructToStructPb(map[string]interface{}{"foo": "MoveThroughJointPositions"}) + test.That(t, err, test.ShouldBeNil) + positionDegs3 := &pb.JointPositions{Values: []float64{1.0, 5.0, 6.0, 1.0, 5.0, 6.0}} + positions := []*pb.JointPositions{positionDegs2, positionDegs3} + positionRads3, err := referenceframe.InputsFromJointPositions(nil, positionDegs3) + test.That(t, err, test.ShouldBeNil) + expectedVelocity := 180. + expectedMoveOptions := &pb.MoveOptions{MaxVelDegsPerSec: &expectedVelocity} + _, err = armServer.MoveThroughJointPositions( + context.Background(), + &pb.MoveThroughJointPositionsRequest{Name: testArmName, Positions: positions, Options: expectedMoveOptions, Extra: ext}, + ) + test.That(t, err, test.ShouldBeNil) + test.That(t, capArmJointPos, test.ShouldResemble, positionRads3) + test.That(t, moveOptions, test.ShouldResemble, arm.MoveOptions{MaxVel: utils.DegToRad(expectedVelocity)}) + test.That(t, extraOptions, test.ShouldResemble, map[string]interface{}{"foo": "MoveThroughJointPositions"}) + }) + t.Run("get kinematics", func(t *testing.T) { _, err = armServer.GetKinematics(context.Background(), &commonpb.GetKinematicsRequest{Name: missingArmName}) test.That(t, err, test.ShouldNotBeNil) diff --git a/services/motion/motion_helpers.go b/services/motion/motion_helpers.go index a16f4e72542..38c0e59b1a2 100644 --- a/services/motion/motion_helpers.go +++ b/services/motion/motion_helpers.go @@ -84,5 +84,5 @@ func MoveArm(ctx context.Context, logger logging.Logger, a arm.Arm, dst spatialm if err != nil { return err } - return a.MoveThroughJointPositions(ctx, plan, arm.NewDefaultMoveOptions(), nil) + return a.MoveThroughJointPositions(ctx, plan, nil, nil) } diff --git a/testutils/inject/arm.go b/testutils/inject/arm.go index df0d92bba3c..8916af39ef3 100644 --- a/testutils/inject/arm.go +++ b/testutils/inject/arm.go @@ -12,19 +12,25 @@ import ( // Arm is an injected arm. type Arm struct { arm.Arm - name resource.Name - DoFunc func(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) - EndPositionFunc func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) - MoveToPositionFunc func(ctx context.Context, to spatialmath.Pose, extra map[string]interface{}) error - MoveToJointPositionsFunc func(ctx context.Context, positions []referenceframe.Input, extra map[string]interface{}) error - JointPositionsFunc func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) - StopFunc func(ctx context.Context, extra map[string]interface{}) error - IsMovingFunc func(context.Context) (bool, error) - CloseFunc func(ctx context.Context) error - ModelFrameFunc func() referenceframe.Model - CurrentInputsFunc func(ctx context.Context) ([]referenceframe.Input, error) - GoToInputsFunc func(ctx context.Context, inputSteps ...[]referenceframe.Input) error - GeometriesFunc func(ctx context.Context) ([]spatialmath.Geometry, error) + name resource.Name + DoFunc func(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) + EndPositionFunc func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) + MoveToPositionFunc func(ctx context.Context, to spatialmath.Pose, extra map[string]interface{}) error + MoveToJointPositionsFunc func(ctx context.Context, positions []referenceframe.Input, extra map[string]interface{}) error + MoveThroughJointPositionsFunc func( + ctx context.Context, + positions [][]referenceframe.Input, + options *arm.MoveOptions, + extra map[string]interface{}, + ) error + JointPositionsFunc func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) + StopFunc func(ctx context.Context, extra map[string]interface{}) error + IsMovingFunc func(context.Context) (bool, error) + CloseFunc func(ctx context.Context) error + ModelFrameFunc func() referenceframe.Model + CurrentInputsFunc func(ctx context.Context) ([]referenceframe.Input, error) + GoToInputsFunc func(ctx context.Context, inputSteps ...[]referenceframe.Input) error + GeometriesFunc func(ctx context.Context) ([]spatialmath.Geometry, error) } // NewArm returns a new injected arm. @@ -61,6 +67,19 @@ func (a *Arm) MoveToJointPositions(ctx context.Context, positions []referencefra return a.MoveToJointPositionsFunc(ctx, positions, extra) } +// MoveToJointPositions calls the injected MoveToJointPositions or the real version. +func (a *Arm) MoveThroughJointPositions( + ctx context.Context, + positions [][]referenceframe.Input, + options *arm.MoveOptions, + extra map[string]interface{}, +) error { + if a.MoveToJointPositionsFunc == nil { + return a.Arm.MoveThroughJointPositions(ctx, positions, options, extra) + } + return a.MoveThroughJointPositionsFunc(ctx, positions, options, extra) +} + // JointPositions calls the injected JointPositions or the real version. func (a *Arm) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { if a.JointPositionsFunc == nil { From 1f41c35990117a7d61be9980ffcaa0c24ef69c89 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Fri, 1 Nov 2024 12:00:58 -0400 Subject: [PATCH 12/14] more fixes --- components/arm/client.go | 8 +------- components/arm/client_test.go | 2 +- components/arm/eva/eva.go | 1 - components/arm/fake/fake.go | 2 +- components/arm/pb_helpers.go | 1 + components/arm/universalrobots/ur.go | 1 - components/arm/wrapper/wrapper.go | 1 + testutils/inject/arm.go | 2 +- 8 files changed, 6 insertions(+), 12 deletions(-) diff --git a/components/arm/client.go b/components/arm/client.go index d872e60a846..9991ead9686 100644 --- a/components/arm/client.go +++ b/components/arm/client.go @@ -172,13 +172,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) { diff --git a/components/arm/client_test.go b/components/arm/client_test.go index 9381e7ec9f1..d9291a10600 100644 --- a/components/arm/client_test.go +++ b/components/arm/client_test.go @@ -174,7 +174,7 @@ 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( + err = arm1Client.MoveThroughJointPositions( context.Background(), [][]referenceframe.Input{jointPos2, jointPos1}, &expectedMoveOptions, diff --git a/components/arm/eva/eva.go b/components/arm/eva/eva.go index 43da89af3f0..8d85334ec7c 100644 --- a/components/arm/eva/eva.go +++ b/components/arm/eva/eva.go @@ -6,7 +6,6 @@ package eva import ( "bytes" "context" - // for embedding model file. _ "embed" "encoding/json" diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index e9ede7f28d1..92947835cf1 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -188,7 +188,7 @@ func (a *Arm) MoveToJointPositions(ctx context.Context, joints []referenceframe. return nil } -// GoToInputs moves the fake arm through the given inputs. +// MoveThroughJointPositions moves the fake arm through the given inputs. func (a *Arm) MoveThroughJointPositions( ctx context.Context, positions [][]referenceframe.Input, diff --git a/components/arm/pb_helpers.go b/components/arm/pb_helpers.go index 70e2cf12d1d..2b39ec1467e 100644 --- a/components/arm/pb_helpers.go +++ b/components/arm/pb_helpers.go @@ -11,6 +11,7 @@ import ( "go.viam.com/rdk/utils" ) +// MoveOptions define parameters to be obeyed during arm movement. type MoveOptions struct { MaxVel, MaxAcc float64 } diff --git a/components/arm/universalrobots/ur.go b/components/arm/universalrobots/ur.go index aa922759001..6ff1304ad01 100644 --- a/components/arm/universalrobots/ur.go +++ b/components/arm/universalrobots/ur.go @@ -4,7 +4,6 @@ package universalrobots import ( "bufio" "context" - // for embedding model file. _ "embed" "encoding/binary" diff --git a/components/arm/wrapper/wrapper.go b/components/arm/wrapper/wrapper.go index e6ab33a6947..16c66d2ac16 100644 --- a/components/arm/wrapper/wrapper.go +++ b/components/arm/wrapper/wrapper.go @@ -135,6 +135,7 @@ func (wrapper *Arm) MoveToJointPositions(ctx context.Context, joints []reference return wrapper.actual.MoveToJointPositions(ctx, joints, extra) } +// MoveThroughJointPositions moves the arm sequentially through the given joints. func (wrapper *Arm) MoveThroughJointPositions( ctx context.Context, positions [][]referenceframe.Input, diff --git a/testutils/inject/arm.go b/testutils/inject/arm.go index 8916af39ef3..765171728b3 100644 --- a/testutils/inject/arm.go +++ b/testutils/inject/arm.go @@ -67,7 +67,7 @@ func (a *Arm) MoveToJointPositions(ctx context.Context, positions []referencefra return a.MoveToJointPositionsFunc(ctx, positions, extra) } -// MoveToJointPositions calls the injected MoveToJointPositions or the real version. +// MoveThroughJointPositions calls the injected MoveThroughJointPositions or the real version. func (a *Arm) MoveThroughJointPositions( ctx context.Context, positions [][]referenceframe.Input, From 891fd1096515e31957973a3a31c287c319cbf30f Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Tue, 5 Nov 2024 11:03:25 -0500 Subject: [PATCH 13/14] rename stuct members --- components/arm/client_test.go | 2 +- components/arm/pb_helpers.go | 10 +++++----- components/arm/server_test.go | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/components/arm/client_test.go b/components/arm/client_test.go index d9291a10600..b55b0e7936f 100644 --- a/components/arm/client_test.go +++ b/components/arm/client_test.go @@ -39,7 +39,7 @@ func TestClient(t *testing.T) { 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{MaxVel: 1, MaxAcc: 2} + 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 diff --git a/components/arm/pb_helpers.go b/components/arm/pb_helpers.go index 2b39ec1467e..3a1106958c2 100644 --- a/components/arm/pb_helpers.go +++ b/components/arm/pb_helpers.go @@ -13,7 +13,7 @@ import ( // MoveOptions define parameters to be obeyed during arm movement. type MoveOptions struct { - MaxVel, MaxAcc float64 + MaxVelRads, MaxAccRads float64 } func moveOptionsFromProtobuf(protobuf *pb.MoveOptions) *MoveOptions { @@ -29,14 +29,14 @@ func moveOptionsFromProtobuf(protobuf *pb.MoveOptions) *MoveOptions { acc = *protobuf.MaxAccDegsPerSec2 } return &MoveOptions{ - MaxVel: utils.DegToRad(vel), - MaxAcc: utils.DegToRad(acc), + MaxVelRads: utils.DegToRad(vel), + MaxAccRads: utils.DegToRad(acc), } } func (opts MoveOptions) toProtobuf() *pb.MoveOptions { - vel := utils.RadToDeg(opts.MaxVel) - acc := utils.RadToDeg(opts.MaxAcc) + vel := utils.RadToDeg(opts.MaxVelRads) + acc := utils.RadToDeg(opts.MaxAccRads) return &pb.MoveOptions{ MaxVelDegsPerSec: &vel, MaxAccDegsPerSec2: &acc, diff --git a/components/arm/server_test.go b/components/arm/server_test.go index 1fa3bb1ab42..8313fbefdf4 100644 --- a/components/arm/server_test.go +++ b/components/arm/server_test.go @@ -236,7 +236,7 @@ func TestServer(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) test.That(t, capArmJointPos, test.ShouldResemble, positionRads3) - test.That(t, moveOptions, test.ShouldResemble, arm.MoveOptions{MaxVel: utils.DegToRad(expectedVelocity)}) + test.That(t, moveOptions, test.ShouldResemble, arm.MoveOptions{MaxVelRads: utils.DegToRad(expectedVelocity)}) test.That(t, extraOptions, test.ShouldResemble, map[string]interface{}{"foo": "MoveThroughJointPositions"}) }) From 47a80bac9e5ba6fd7011d89edfff4a5421335e2f Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Tue, 5 Nov 2024 12:28:37 -0500 Subject: [PATCH 14/14] respond to PR comments --- components/arm/client.go | 9 ++++++--- components/arm/pb_helpers.go | 2 +- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/components/arm/client.go b/components/arm/client.go index 9991ead9686..66e26e6619b 100644 --- a/components/arm/client.go +++ b/components/arm/client.go @@ -127,12 +127,15 @@ func (c *client) MoveThroughJointPositions( } allJPs = append(allJPs, jp) } - _, err = c.client.MoveThroughJointPositions(ctx, &pb.MoveThroughJointPositionsRequest{ + req := &pb.MoveThroughJointPositionsRequest{ Name: c.name, Positions: allJPs, - Options: options.toProtobuf(), Extra: ext, - }) + } + if options != nil { + req.Options = options.toProtobuf() + } + _, err = c.client.MoveThroughJointPositions(ctx, req) return err } diff --git a/components/arm/pb_helpers.go b/components/arm/pb_helpers.go index 3a1106958c2..e8668ffbaa1 100644 --- a/components/arm/pb_helpers.go +++ b/components/arm/pb_helpers.go @@ -34,7 +34,7 @@ func moveOptionsFromProtobuf(protobuf *pb.MoveOptions) *MoveOptions { } } -func (opts MoveOptions) toProtobuf() *pb.MoveOptions { +func (opts *MoveOptions) toProtobuf() *pb.MoveOptions { vel := utils.RadToDeg(opts.MaxVelRads) acc := utils.RadToDeg(opts.MaxAccRads) return &pb.MoveOptions{