diff --git a/components/arm/arm.go b/components/arm/arm.go index 38008b3ac12..c14cccc3cc9 100644 --- a/components/arm/arm.go +++ b/components/arm/arm.go @@ -7,7 +7,6 @@ import ( "sync" "github.com/edaniels/golog" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/arm/v1" viamutils "go.viam.com/utils" "go.viam.com/utils/rpc" @@ -85,7 +84,7 @@ type Arm interface { // MoveToPosition moves the arm to the given absolute position. // The worldState argument should be treated as optional by all implementing drivers // This will block until done or a new operation cancels this one - MoveToPosition(ctx context.Context, pose spatialmath.Pose, worldState *commonpb.WorldState, extra map[string]interface{}) error + MoveToPosition(ctx context.Context, pose spatialmath.Pose, worldState *referenceframe.WorldState, 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 @@ -219,7 +218,7 @@ func (r *reconfigurableArm) EndPosition(ctx context.Context, extra map[string]in func (r *reconfigurableArm) MoveToPosition( ctx context.Context, pose spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { r.mu.RLock() @@ -350,7 +349,7 @@ func WrapWithReconfigurable(r interface{}, name resource.Name) (resource.Reconfi } // Move is a helper function to abstract away movement for general arms. -func Move(ctx context.Context, r robot.Robot, a Arm, dst spatialmath.Pose, worldState *commonpb.WorldState) error { +func Move(ctx context.Context, r robot.Robot, a Arm, dst spatialmath.Pose, worldState *referenceframe.WorldState) error { solution, err := Plan(ctx, r, a, dst, worldState) if err != nil { return err @@ -365,10 +364,10 @@ func Plan( r robot.Robot, a Arm, dst spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, ) ([][]referenceframe.Input, error) { // build the framesystem - fs, err := framesystem.RobotFrameSystem(ctx, r, worldState.GetTransforms()) + fs, err := framesystem.RobotFrameSystem(ctx, r, worldState.Transforms) if err != nil { return nil, err } diff --git a/components/arm/client.go b/components/arm/client.go index ce9c5efa548..4367e453371 100644 --- a/components/arm/client.go +++ b/components/arm/client.go @@ -6,7 +6,6 @@ import ( "errors" "github.com/edaniels/golog" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/arm/v1" "go.viam.com/utils/protoutils" "go.viam.com/utils/rpc" @@ -55,17 +54,21 @@ func (c *client) EndPosition(ctx context.Context, extra map[string]interface{}) func (c *client) MoveToPosition( ctx context.Context, pose spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { ext, err := protoutils.StructToStructPb(extra) if err != nil { return err } + worldStateMsg, err := referenceframe.WorldStateToProtobuf(worldState) + if err != nil { + return err + } _, err = c.client.MoveToPosition(ctx, &pb.MoveToPositionRequest{ Name: c.name, To: spatialmath.PoseToProtobuf(pose), - WorldState: worldState, + WorldState: worldStateMsg, Extra: ext, }) return err diff --git a/components/arm/client_test.go b/components/arm/client_test.go index 5c40d914f84..6ca79a0445c 100644 --- a/components/arm/client_test.go +++ b/components/arm/client_test.go @@ -7,7 +7,6 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" - commonpb "go.viam.com/api/common/v1" componentpb "go.viam.com/api/component/arm/v1" "go.viam.com/test" "go.viam.com/utils" @@ -17,6 +16,7 @@ import ( "go.viam.com/rdk/components/arm" "go.viam.com/rdk/components/generic" viamgrpc "go.viam.com/rdk/grpc" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/registry" "go.viam.com/rdk/resource" "go.viam.com/rdk/spatialmath" @@ -52,7 +52,7 @@ func TestClient(t *testing.T) { injectArm.MoveToPositionFunc = func( ctx context.Context, ap spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { capArmPos = ap @@ -82,7 +82,7 @@ func TestClient(t *testing.T) { injectArm2.MoveToPositionFunc = func( ctx context.Context, ap spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { capArmPos = ap @@ -139,7 +139,7 @@ func TestClient(t *testing.T) { test.That(t, jointPos.String(), test.ShouldResemble, jointPos1.String()) test.That(t, extraOptions, test.ShouldResemble, map[string]interface{}{"foo": "JointPositions"}) - err = arm1Client.MoveToPosition(context.Background(), pos2, &commonpb.WorldState{}, map[string]interface{}{"foo": "MoveToPosition"}) + err = arm1Client.MoveToPosition(context.Background(), pos2, &referenceframe.WorldState{}, map[string]interface{}{"foo": "MoveToPosition"}) test.That(t, err, test.ShouldBeNil) test.That(t, spatialmath.PoseAlmostEqual(capArmPos, pos2), test.ShouldBeTrue) diff --git a/components/arm/eva/eva.go b/components/arm/eva/eva.go index f1efe215e33..39bc243011b 100644 --- a/components/arm/eva/eva.go +++ b/components/arm/eva/eva.go @@ -17,7 +17,6 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" "go.uber.org/multierr" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/arm/v1" "go.viam.com/utils" @@ -123,7 +122,7 @@ func (e *eva) EndPosition(ctx context.Context, extra map[string]interface{}) (sp func (e *eva) MoveToPosition( ctx context.Context, pos spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { ctx, done := e.opMgr.New(ctx) diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index 1579cca328e..77094abeba7 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -8,7 +8,6 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/arm/v1" "go.viam.com/rdk/components/arm" @@ -133,7 +132,7 @@ func (a *Arm) EndPosition(ctx context.Context, extra map[string]interface{}) (sp func (a *Arm) MoveToPosition( ctx context.Context, pos spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { joints, err := a.JointPositions(ctx, extra) diff --git a/components/arm/server.go b/components/arm/server.go index ec814c02876..98ad9a7e494 100644 --- a/components/arm/server.go +++ b/components/arm/server.go @@ -8,6 +8,7 @@ import ( pb "go.viam.com/api/component/arm/v1" "go.viam.com/rdk/operation" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/subtype" ) @@ -76,10 +77,14 @@ func (s *subtypeServer) MoveToPosition(ctx context.Context, req *pb.MoveToPositi if err != nil { return nil, err } + worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState()) + if err != nil { + return nil, err + } return &pb.MoveToPositionResponse{}, arm.MoveToPosition( ctx, spatialmath.NewPoseFromProtobuf(req.GetTo()), - req.GetWorldState(), + worldState, req.Extra.AsMap(), ) } diff --git a/components/arm/server_test.go b/components/arm/server_test.go index e9c866a017d..60dc0b55392 100644 --- a/components/arm/server_test.go +++ b/components/arm/server_test.go @@ -12,6 +12,7 @@ import ( "go.viam.com/utils/protoutils" "go.viam.com/rdk/components/arm" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/subtype" @@ -56,7 +57,7 @@ func TestServer(t *testing.T) { injectArm.MoveToPositionFunc = func( ctx context.Context, ap spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { capArmPos = ap @@ -85,7 +86,7 @@ func TestServer(t *testing.T) { injectArm2.MoveToPositionFunc = func( ctx context.Context, ap spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { capArmPos = ap diff --git a/components/arm/trossen/trossen.go b/components/arm/trossen/trossen.go index 69435920a10..aa236fa8ba9 100644 --- a/components/arm/trossen/trossen.go +++ b/components/arm/trossen/trossen.go @@ -13,7 +13,6 @@ import ( "github.com/edaniels/golog" "github.com/jacobsa/go-serial/serial" "github.com/pkg/errors" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/arm/v1" "go.viam.com/dynamixel/network" "go.viam.com/dynamixel/servo" @@ -203,7 +202,7 @@ func (a *Arm) EndPosition(ctx context.Context, extra map[string]interface{}) (sp func (a *Arm) MoveToPosition( ctx context.Context, pos spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { ctx, done := a.opMgr.New(ctx) diff --git a/components/arm/universalrobots/ur.go b/components/arm/universalrobots/ur.go index 890e56a81fe..1fa3db53a8d 100644 --- a/components/arm/universalrobots/ur.go +++ b/components/arm/universalrobots/ur.go @@ -16,7 +16,6 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" "go.uber.org/multierr" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/arm/v1" goutils "go.viam.com/utils" @@ -257,7 +256,7 @@ func (ua *URArm) EndPosition(ctx context.Context, extra map[string]interface{}) func (ua *URArm) MoveToPosition( ctx context.Context, pos spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { ctx, done := ua.opMgr.New(ctx) @@ -473,7 +472,7 @@ func reader(ctx context.Context, conn io.Reader, ua *URArm, onHaveData func()) e const errURHostedKinematics = "cannot use UR hosted kinematics with obstacles or interaction spaces" -func (ua *URArm) useURHostedKinematics(worldState *commonpb.WorldState, extra map[string]interface{}) (bool, error) { +func (ua *URArm) useURHostedKinematics(worldState *referenceframe.WorldState, extra map[string]interface{}) (bool, error) { // function to error out if trying to use world state with hosted kinematics checkWorldState := func(usingHostedKinematics bool) (bool, error) { if usingHostedKinematics && worldState != nil && (len(worldState.Obstacles) != 0 || len(worldState.InteractionSpaces) != 0) { diff --git a/components/arm/universalrobots/ur5e_test.go b/components/arm/universalrobots/ur5e_test.go index 0b3fe4215aa..b32b6b4e4ea 100644 --- a/components/arm/universalrobots/ur5e_test.go +++ b/components/arm/universalrobots/ur5e_test.go @@ -9,7 +9,6 @@ import ( "github.com/edaniels/golog" "github.com/go-gl/mathgl/mgl64" "github.com/golang/geo/r3" - commonpb "go.viam.com/api/common/v1" "go.viam.com/test" "gonum.org/v1/gonum/mat" "gonum.org/v1/gonum/num/quat" @@ -134,14 +133,15 @@ func TestKin1(t *testing.T) { } func TestUseURHostedKinematics(t *testing.T) { - gifs := []*commonpb.GeometriesInFrame{{Geometries: []*commonpb.Geometry{{ - Center: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), - GeometryType: &commonpb.Geometry_Sphere{Sphere: &commonpb.Sphere{RadiusMm: 1}}, - }}}} + sphere, err := spatialmath.NewSphere(r3.Vector{}, 1, "") + test.That(t, err, test.ShouldBeNil) + obstacles := make(map[string]spatialmath.Geometry) + obstacles["sphere"] = sphere + gifs := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, obstacles)} // test that under normal circumstances we can use worldstate and our own kinematics ur := URArm{} - using, err := ur.useURHostedKinematics(&commonpb.WorldState{Obstacles: gifs}, nil) + using, err := ur.useURHostedKinematics(&referenceframe.WorldState{Obstacles: gifs}, nil) test.That(t, err, test.ShouldBeNil) test.That(t, using, test.ShouldBeFalse) @@ -154,23 +154,23 @@ func TestUseURHostedKinematics(t *testing.T) { // test specifying at config time with no obstacles or extra params at runtime ur.urHostedKinematics = true - using, err = ur.useURHostedKinematics(&commonpb.WorldState{}, nil) + using, err = ur.useURHostedKinematics(&referenceframe.WorldState{}, nil) test.That(t, err, test.ShouldBeNil) test.That(t, using, test.ShouldBeTrue) // test that we can override the config preference with extra params extraParams["arm_hosted_kinematics"] = false - using, err = ur.useURHostedKinematics(&commonpb.WorldState{Obstacles: gifs}, extraParams) + using, err = ur.useURHostedKinematics(&referenceframe.WorldState{Obstacles: gifs}, extraParams) test.That(t, err, test.ShouldBeNil) test.That(t, using, test.ShouldBeFalse) // test obstacles will cause this to error - _, err = ur.useURHostedKinematics(&commonpb.WorldState{Obstacles: gifs}, nil) + _, err = ur.useURHostedKinematics(&referenceframe.WorldState{Obstacles: gifs}, nil) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldResemble, errURHostedKinematics) // test obstacles will cause this to error - _, err = ur.useURHostedKinematics(&commonpb.WorldState{InteractionSpaces: gifs}, nil) + _, err = ur.useURHostedKinematics(&referenceframe.WorldState{InteractionSpaces: gifs}, nil) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldResemble, errURHostedKinematics) } diff --git a/components/arm/wrapper/wrapper.go b/components/arm/wrapper/wrapper.go index 251862e784e..525f506943a 100644 --- a/components/arm/wrapper/wrapper.go +++ b/components/arm/wrapper/wrapper.go @@ -5,7 +5,6 @@ import ( "context" "github.com/edaniels/golog" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/arm/v1" "go.viam.com/utils" @@ -103,7 +102,7 @@ func (wrapper *Arm) EndPosition(ctx context.Context, extra map[string]interface{ func (wrapper *Arm) MoveToPosition( ctx context.Context, pos spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { ctx, done := wrapper.opMgr.New(ctx) diff --git a/components/arm/xarm/xarm_comm.go b/components/arm/xarm/xarm_comm.go index 5e21ffa4093..a82df7a1b60 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" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/arm/v1" "go.viam.com/utils" @@ -399,7 +398,7 @@ func (x *xArm) EndPosition(ctx context.Context, extra map[string]interface{}) (s func (x *xArm) MoveToPosition( ctx context.Context, pos spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { ctx, done := x.opMgr.New(ctx) diff --git a/components/arm/yahboom/dofbot.go b/components/arm/yahboom/dofbot.go index d3a520021ae..f1467f67d20 100644 --- a/components/arm/yahboom/dofbot.go +++ b/components/arm/yahboom/dofbot.go @@ -12,7 +12,6 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" - commonpb "go.viam.com/api/common/v1" componentpb "go.viam.com/api/component/arm/v1" "go.viam.com/utils" @@ -176,7 +175,7 @@ func (a *Dofbot) EndPosition(ctx context.Context, extra map[string]interface{}) func (a *Dofbot) MoveToPosition( ctx context.Context, pos spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { ctx, done := a.opMgr.New(ctx) diff --git a/components/camera/videosource/join_pointcloud_test.go b/components/camera/videosource/join_pointcloud_test.go index 528116b0024..aa1a829027b 100644 --- a/components/camera/videosource/join_pointcloud_test.go +++ b/components/camera/videosource/join_pointcloud_test.go @@ -10,7 +10,6 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" - commonpb "go.viam.com/api/common/v1" "go.viam.com/test" "go.viam.com/utils" "go.viam.com/utils/artifact" @@ -93,7 +92,8 @@ func makeFakeRobot(t *testing.T) robot.Robot { }, } r.FrameSystemConfigFunc = func( - ctx context.Context, additionalTransforms []*commonpb.Transform, + ctx context.Context, + additionalTransforms []*referenceframe.PoseInFrame, ) (framesystemparts.Parts, error) { return fsParts, nil } @@ -349,7 +349,8 @@ func makeFakeRobotICP(t *testing.T) (robot.Robot, error) { }, } r.FrameSystemConfigFunc = func( - ctx context.Context, additionalTransforms []*commonpb.Transform, + ctx context.Context, + additionalTransforms []*referenceframe.PoseInFrame, ) (framesystemparts.Parts, error) { return fsParts, nil } diff --git a/components/gantry/client.go b/components/gantry/client.go index 5cbd8fc1f6b..f077e249b0f 100644 --- a/components/gantry/client.go +++ b/components/gantry/client.go @@ -5,7 +5,6 @@ import ( "context" "github.com/edaniels/golog" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/gantry/v1" "go.viam.com/utils/protoutils" "go.viam.com/utils/rpc" @@ -66,17 +65,21 @@ func (c *client) Lengths(ctx context.Context, extra map[string]interface{}) ([]f func (c *client) MoveToPosition( ctx context.Context, positionsMm []float64, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { ext, err := protoutils.StructToStructPb(extra) if err != nil { return err } + worldStateMsg, err := referenceframe.WorldStateToProtobuf(worldState) + if err != nil { + return err + } _, err = c.client.MoveToPosition(ctx, &pb.MoveToPositionRequest{ Name: c.name, PositionsMm: positionsMm, - WorldState: worldState, + WorldState: worldStateMsg, Extra: ext, }) return err @@ -105,7 +108,7 @@ func (c *client) CurrentInputs(ctx context.Context) ([]referenceframe.Input, err } func (c *client) GoToInputs(ctx context.Context, goal []referenceframe.Input) error { - return c.MoveToPosition(ctx, referenceframe.InputsToFloats(goal), &commonpb.WorldState{}, nil) + return c.MoveToPosition(ctx, referenceframe.InputsToFloats(goal), &referenceframe.WorldState{}, nil) } func (c *client) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { diff --git a/components/gantry/client_test.go b/components/gantry/client_test.go index 50d33556378..2993c85d9f1 100644 --- a/components/gantry/client_test.go +++ b/components/gantry/client_test.go @@ -7,7 +7,6 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" - commonpb "go.viam.com/api/common/v1" componentpb "go.viam.com/api/component/gantry/v1" "go.viam.com/test" "go.viam.com/utils" @@ -17,6 +16,7 @@ import ( "go.viam.com/rdk/components/gantry" "go.viam.com/rdk/components/generic" viamgrpc "go.viam.com/rdk/grpc" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/registry" "go.viam.com/rdk/resource" "go.viam.com/rdk/subtype" @@ -44,7 +44,7 @@ func TestClient(t *testing.T) { injectGantry.MoveToPositionFunc = func( ctx context.Context, pos []float64, - worldState *commonpb.WorldState, + ws *referenceframe.WorldState, extra map[string]interface{}, ) error { gantryPos = pos @@ -71,7 +71,7 @@ func TestClient(t *testing.T) { injectGantry2.MoveToPositionFunc = func( ctx context.Context, pos []float64, - worldState *commonpb.WorldState, + ws *referenceframe.WorldState, extra map[string]interface{}, ) error { gantryPos = pos @@ -126,7 +126,12 @@ func TestClient(t *testing.T) { test.That(t, pos, test.ShouldResemble, pos1) test.That(t, extra1, test.ShouldResemble, map[string]interface{}{"foo": 123., "bar": "234"}) - err = gantry1Client.MoveToPosition(context.Background(), pos2, &commonpb.WorldState{}, map[string]interface{}{"foo": 234, "bar": "345"}) + err = gantry1Client.MoveToPosition( + context.Background(), + pos2, + &referenceframe.WorldState{}, + map[string]interface{}{"foo": 234, "bar": "345"}, + ) test.That(t, err, test.ShouldBeNil) test.That(t, gantryPos, test.ShouldResemble, pos2) test.That(t, extra1, test.ShouldResemble, map[string]interface{}{"foo": 234., "bar": "345"}) diff --git a/components/gantry/fake/gantry.go b/components/gantry/fake/gantry.go index 1c9d4814005..59323ec774d 100644 --- a/components/gantry/fake/gantry.go +++ b/components/gantry/fake/gantry.go @@ -7,7 +7,6 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" - commonpb "go.viam.com/api/common/v1" "go.viam.com/rdk/components/gantry" "go.viam.com/rdk/components/generic" @@ -53,7 +52,7 @@ func (g *Gantry) Lengths(ctx context.Context, extra map[string]interface{}) ([]f func (g *Gantry) MoveToPosition( ctx context.Context, positionsMm []float64, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { g.positionsMm = positionsMm @@ -92,5 +91,5 @@ func (g *Gantry) CurrentInputs(ctx context.Context) ([]referenceframe.Input, err // GoToInputs moves using the Gantry frames.. func (g *Gantry) GoToInputs(ctx context.Context, goal []referenceframe.Input) error { - return g.MoveToPosition(ctx, referenceframe.InputsToFloats(goal), &commonpb.WorldState{}, nil) + return g.MoveToPosition(ctx, referenceframe.InputsToFloats(goal), &referenceframe.WorldState{}, nil) } diff --git a/components/gantry/gantry.go b/components/gantry/gantry.go index ffa42f0686b..08f325d845f 100644 --- a/components/gantry/gantry.go +++ b/components/gantry/gantry.go @@ -5,7 +5,6 @@ import ( "sync" "github.com/edaniels/golog" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/gantry/v1" viamutils "go.viam.com/utils" "go.viam.com/utils/rpc" @@ -72,7 +71,7 @@ type Gantry interface { // MoveToPosition is in meters // The worldState argument should be treated as optional by all implementing drivers // This will block until done or a new operation cancels this one - MoveToPosition(ctx context.Context, positionsMm []float64, worldState *commonpb.WorldState, extra map[string]interface{}) error + MoveToPosition(ctx context.Context, positionsMm []float64, worldState *referenceframe.WorldState, extra map[string]interface{}) error // Lengths is the length of gantries in meters Lengths(ctx context.Context, extra map[string]interface{}) ([]float64, error) @@ -231,7 +230,7 @@ func (g *reconfigurableGantry) Lengths(ctx context.Context, extra map[string]int func (g *reconfigurableGantry) MoveToPosition( ctx context.Context, positionsMm []float64, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { g.mu.Lock() diff --git a/components/gantry/multiaxis/multiaxis.go b/components/gantry/multiaxis/multiaxis.go index 1187f9172c0..25b1209e727 100644 --- a/components/gantry/multiaxis/multiaxis.go +++ b/components/gantry/multiaxis/multiaxis.go @@ -7,7 +7,6 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" - commonpb "go.viam.com/api/common/v1" "go.viam.com/utils" "go.viam.com/rdk/components/gantry" @@ -102,7 +101,7 @@ func newMultiAxis( func (g *multiAxis) MoveToPosition( ctx context.Context, positions []float64, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { ctx, done := g.opMgr.New(ctx) @@ -143,7 +142,7 @@ func (g *multiAxis) GoToInputs(ctx context.Context, goal []referenceframe.Input) return err } - err = subAx.MoveToPosition(ctx, referenceframe.InputsToFloats(goal[idx:idx+len(subAxNum)-1]), &commonpb.WorldState{}, nil) + err = subAx.MoveToPosition(ctx, referenceframe.InputsToFloats(goal[idx:idx+len(subAxNum)-1]), &referenceframe.WorldState{}, nil) if err != nil { return err } diff --git a/components/gantry/multiaxis/multiaxis_test.go b/components/gantry/multiaxis/multiaxis_test.go index 380d86fd515..276c6cd670f 100644 --- a/components/gantry/multiaxis/multiaxis_test.go +++ b/components/gantry/multiaxis/multiaxis_test.go @@ -5,7 +5,6 @@ import ( "testing" "github.com/edaniels/golog" - commonpb "go.viam.com/api/common/v1" "go.viam.com/test" "go.viam.com/rdk/components/gantry" @@ -23,7 +22,7 @@ func createFakeOneaAxis(length float64, positions []float64) *inject.Gantry { PositionFunc: func(ctx context.Context, extra map[string]interface{}) ([]float64, error) { return positions, nil }, - MoveToPositionFunc: func(ctx context.Context, positions []float64, worldState *commonpb.WorldState, extra map[string]interface{}) error { + MoveToPositionFunc: func(ctx context.Context, pos []float64, ws *referenceframe.WorldState, extra map[string]interface{}) error { return nil }, LengthsFunc: func(ctx context.Context, extra map[string]interface{}) ([]float64, error) { @@ -114,17 +113,17 @@ func TestMoveToPosition(t *testing.T) { positions := []float64{} fakemultiaxis := &multiAxis{} - err := fakemultiaxis.MoveToPosition(ctx, positions, &commonpb.WorldState{}, nil) + err := fakemultiaxis.MoveToPosition(ctx, positions, &referenceframe.WorldState{}, nil) test.That(t, err, test.ShouldNotBeNil) fakemultiaxis = &multiAxis{subAxes: threeAxes} positions = []float64{1, 2, 3} - err = fakemultiaxis.MoveToPosition(ctx, positions, &commonpb.WorldState{}, nil) + err = fakemultiaxis.MoveToPosition(ctx, positions, &referenceframe.WorldState{}, nil) test.That(t, err, test.ShouldBeNil) fakemultiaxis = &multiAxis{subAxes: twoAxes} positions = []float64{1, 2} - err = fakemultiaxis.MoveToPosition(ctx, positions, &commonpb.WorldState{}, nil) + err = fakemultiaxis.MoveToPosition(ctx, positions, &referenceframe.WorldState{}, nil) test.That(t, err, test.ShouldBeNil) } diff --git a/components/gantry/oneaxis/oneaxis.go b/components/gantry/oneaxis/oneaxis.go index 102325435c6..fdb12a27583 100644 --- a/components/gantry/oneaxis/oneaxis.go +++ b/components/gantry/oneaxis/oneaxis.go @@ -10,7 +10,6 @@ import ( "github.com/golang/geo/r3" "github.com/pkg/errors" "go.uber.org/multierr" - commonpb "go.viam.com/api/common/v1" utils "go.viam.com/utils" "go.viam.com/rdk/components/board" @@ -384,7 +383,7 @@ func (g *oneAxis) Lengths(ctx context.Context, extra map[string]interface{}) ([] func (g *oneAxis) MoveToPosition( ctx context.Context, positions []float64, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { ctx, done := g.opMgr.New(ctx) @@ -482,5 +481,5 @@ func (g *oneAxis) CurrentInputs(ctx context.Context) ([]referenceframe.Input, er // GoToInputs moves the gantry to a goal position in the Gantry frame. func (g *oneAxis) GoToInputs(ctx context.Context, goal []referenceframe.Input) error { - return g.MoveToPosition(ctx, referenceframe.InputsToFloats(goal), &commonpb.WorldState{}, nil) + return g.MoveToPosition(ctx, referenceframe.InputsToFloats(goal), &referenceframe.WorldState{}, nil) } diff --git a/components/gantry/oneaxis/oneaxis_test.go b/components/gantry/oneaxis/oneaxis_test.go index c9fc45c0dc2..9f334205226 100644 --- a/components/gantry/oneaxis/oneaxis_test.go +++ b/components/gantry/oneaxis/oneaxis_test.go @@ -7,7 +7,6 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" "github.com/pkg/errors" - commonpb "go.viam.com/api/common/v1" "go.viam.com/test" "go.viam.com/rdk/components/board" @@ -626,24 +625,24 @@ func TestMoveToPosition(t *testing.T) { limitHigh: true, } pos := []float64{1, 2} - err := fakegantry.MoveToPosition(ctx, pos, &commonpb.WorldState{}, nil) + err := fakegantry.MoveToPosition(ctx, pos, &referenceframe.WorldState{}, nil) test.That(t, err.Error(), test.ShouldEqual, "oneAxis gantry MoveToPosition needs 1 position, got: 2") pos = []float64{1} - err = fakegantry.MoveToPosition(ctx, pos, &commonpb.WorldState{}, nil) + err = fakegantry.MoveToPosition(ctx, pos, &referenceframe.WorldState{}, nil) test.That(t, err.Error(), test.ShouldEqual, "oneAxis gantry position out of range, got 1.00 max is 0.00") fakegantry.lengthMm = float64(4) fakegantry.positionLimits = []float64{0, 4} fakegantry.limitSwitchPins = []string{"1", "2"} - err = fakegantry.MoveToPosition(ctx, pos, &commonpb.WorldState{}, nil) + err = fakegantry.MoveToPosition(ctx, pos, &referenceframe.WorldState{}, nil) test.That(t, err, test.ShouldBeNil) fakegantry.lengthMm = float64(4) fakegantry.positionLimits = []float64{0.01, .01} fakegantry.limitSwitchPins = []string{"1", "2"} fakegantry.motor = &inject.Motor{StopFunc: func(ctx context.Context, extra map[string]interface{}) error { return errors.New("err") }} - err = fakegantry.MoveToPosition(ctx, pos, &commonpb.WorldState{}, nil) + err = fakegantry.MoveToPosition(ctx, pos, &referenceframe.WorldState{}, nil) test.That(t, err, test.ShouldNotBeNil) injectGPIOPin := &inject.GPIOPin{} @@ -662,7 +661,7 @@ func TestMoveToPosition(t *testing.T) { } fakegantry.board = &inject.Board{GPIOPinByNameFunc: func(pin string) (board.GPIOPin, error) { return injectGPIOPin, nil }} - err = fakegantry.MoveToPosition(ctx, pos, &commonpb.WorldState{}, nil) + err = fakegantry.MoveToPosition(ctx, pos, &referenceframe.WorldState{}, nil) test.That(t, err, test.ShouldNotBeNil) fakegantry.board = &inject.Board{GPIOPinByNameFunc: func(pin string) (board.GPIOPin, error) { return injectGPIOPinGood, nil }} @@ -672,13 +671,13 @@ func TestMoveToPosition(t *testing.T) { return errors.New("err") }, } - err = fakegantry.MoveToPosition(ctx, pos, &commonpb.WorldState{}, nil) + err = fakegantry.MoveToPosition(ctx, pos, &referenceframe.WorldState{}, nil) test.That(t, err, test.ShouldNotBeNil) fakegantry.motor = &inject.Motor{GoToFunc: func(ctx context.Context, rpm, rotations float64, extra map[string]interface{}) error { return nil }} - err = fakegantry.MoveToPosition(ctx, pos, &commonpb.WorldState{}, nil) + err = fakegantry.MoveToPosition(ctx, pos, &referenceframe.WorldState{}, nil) test.That(t, err, test.ShouldBeNil) } diff --git a/components/gantry/server.go b/components/gantry/server.go index 316f48e4f05..7ecf8d1e9e4 100644 --- a/components/gantry/server.go +++ b/components/gantry/server.go @@ -8,6 +8,7 @@ import ( pb "go.viam.com/api/component/gantry/v1" "go.viam.com/rdk/operation" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/subtype" ) @@ -77,7 +78,11 @@ func (s *subtypeServer) MoveToPosition( if err != nil { return nil, err } - return &pb.MoveToPositionResponse{}, gantry.MoveToPosition(ctx, req.PositionsMm, req.GetWorldState(), req.Extra.AsMap()) + worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState()) + if err != nil { + return nil, err + } + return &pb.MoveToPositionResponse{}, gantry.MoveToPosition(ctx, req.PositionsMm, worldState, req.Extra.AsMap()) } // Stop stops the gantry specified. diff --git a/components/gantry/server_test.go b/components/gantry/server_test.go index 707a9baf422..0d7fe661923 100644 --- a/components/gantry/server_test.go +++ b/components/gantry/server_test.go @@ -5,12 +5,12 @@ import ( "testing" "github.com/pkg/errors" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/gantry/v1" "go.viam.com/test" "go.viam.com/utils/protoutils" "go.viam.com/rdk/components/gantry" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/subtype" "go.viam.com/rdk/testutils/inject" @@ -47,7 +47,7 @@ func TestServer(t *testing.T) { injectGantry.MoveToPositionFunc = func( ctx context.Context, pos []float64, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { gantryPos = pos @@ -70,7 +70,7 @@ func TestServer(t *testing.T) { injectGantry2.MoveToPositionFunc = func( ctx context.Context, pos []float64, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { gantryPos = pos diff --git a/config/frame_system.go b/config/frame_system.go index cc48a67c0c2..70f1be2b3dd 100644 --- a/config/frame_system.go +++ b/config/frame_system.go @@ -66,33 +66,18 @@ func ProtobufToFrameSystemPart(fsc *pb.FrameSystemConfig) (*FrameSystemPart, err return part, nil } -// NewMissingReferenceFrameError returns an error indicating that a particular -// protobuf message is missing necessary information for its ReferenceFrame key. -func NewMissingReferenceFrameError(msg interface{}) error { - return errors.Errorf("missing reference frame in protobuf message of type %T", msg) -} - -// ConvertTransformProtobufToFrameSystemPart creates a FrameSystem part out of a -// transform protobuf message. -func ConvertTransformProtobufToFrameSystemPart(transformMsg *commonpb.Transform) (*FrameSystemPart, error) { - frameName := transformMsg.GetReferenceFrame() - if frameName == "" { - return nil, NewMissingReferenceFrameError(transformMsg) - } - poseInObserverFrame := transformMsg.GetPoseInObserverFrame() - parentFrame := poseInObserverFrame.GetReferenceFrame() - if parentFrame == "" { - return nil, NewMissingReferenceFrameError(poseInObserverFrame) +// PoseInFrameToFrameSystemPart creates a FrameSystem part out of a PoseInFrame. +func PoseInFrameToFrameSystemPart(transform *referenceframe.PoseInFrame) (*FrameSystemPart, error) { + if transform.Name() == "" || transform.FrameName() == "" { + return nil, referenceframe.ErrEmptyStringFrameName } - poseMsg := poseInObserverFrame.GetPose() - pose := spatialmath.NewPoseFromProtobuf(poseMsg) frameConfig := &Frame{ - Parent: parentFrame, - Translation: pose.Point(), - Orientation: pose.Orientation(), + Parent: transform.FrameName(), + Translation: transform.Pose().Point(), + Orientation: transform.Pose().Orientation(), } part := &FrameSystemPart{ - Name: frameName, + Name: transform.Name(), FrameConfig: frameConfig, } return part, nil diff --git a/config/frame_system_test.go b/config/frame_system_test.go index 9c1ee5821f7..33540a3eeab 100644 --- a/config/frame_system_test.go +++ b/config/frame_system_test.go @@ -137,52 +137,20 @@ func TestFramesFromPart(t *testing.T) { } func TestConvertTransformProtobufToFrameSystemPart(t *testing.T) { - zeroPose := spatial.PoseToProtobuf(spatial.NewZeroPose()) t.Run("fails on missing reference frame name", func(t *testing.T) { - transform := &commonpb.Transform{ - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "parent", - Pose: zeroPose, - }, - } - part, err := ConvertTransformProtobufToFrameSystemPart(transform) - test.That(t, err, test.ShouldBeError, NewMissingReferenceFrameError(&commonpb.Transform{})) - test.That(t, part, test.ShouldBeNil) - }) - t.Run("fails on missing reference frame name", func(t *testing.T) { - transform := &commonpb.Transform{ - ReferenceFrame: "child", - PoseInObserverFrame: &commonpb.PoseInFrame{ - Pose: zeroPose, - }, - } - part, err := ConvertTransformProtobufToFrameSystemPart(transform) - test.That(t, err, test.ShouldBeError, NewMissingReferenceFrameError(&commonpb.PoseInFrame{})) + transform := referenceframe.NewPoseInFrame("parent", spatial.NewZeroPose()) + part, err := PoseInFrameToFrameSystemPart(transform) + test.That(t, err, test.ShouldBeError, referenceframe.ErrEmptyStringFrameName) test.That(t, part, test.ShouldBeNil) }) t.Run("converts to frame system part", func(t *testing.T) { - testPose := spatial.NewPoseFromOrientation( - r3.Vector{X: 1., Y: 2., Z: 3.}, - &spatial.R4AA{Theta: math.Pi / 2, RX: 0, RY: 1, RZ: 0}, - ) - transform := &commonpb.Transform{ - ReferenceFrame: "child", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "parent", - Pose: spatial.PoseToProtobuf(testPose), - }, - } - transformPOF := transform.GetPoseInObserverFrame() - posePt := testPose.Point() - part, err := ConvertTransformProtobufToFrameSystemPart(transform) + testPose := spatial.NewPoseFromOrientation(r3.Vector{X: 1., Y: 2., Z: 3.}, &spatial.R4AA{Theta: math.Pi / 2, RX: 0, RY: 1, RZ: 0}) + transform := referenceframe.NewNamedPoseInFrame("parent", testPose, "child") + part, err := PoseInFrameToFrameSystemPart(transform) test.That(t, err, test.ShouldBeNil) - test.That(t, part.Name, test.ShouldEqual, transform.GetReferenceFrame()) - test.That(t, part.FrameConfig.Parent, test.ShouldEqual, transformPOF.GetReferenceFrame()) - partTrans := part.FrameConfig.Translation - partOrient := part.FrameConfig.Orientation - test.That(t, partTrans.X, test.ShouldAlmostEqual, posePt.X) - test.That(t, partTrans.Y, test.ShouldAlmostEqual, posePt.Y) - test.That(t, partTrans.Z, test.ShouldAlmostEqual, posePt.Z) - test.That(t, spatial.OrientationAlmostEqual(partOrient, testPose.Orientation()), test.ShouldBeTrue) + test.That(t, part.Name, test.ShouldEqual, transform.Name()) + test.That(t, part.FrameConfig.Parent, test.ShouldEqual, transform.FrameName()) + test.That(t, spatial.R3VectorAlmostEqual(part.FrameConfig.Translation, testPose.Point(), 1e-8), test.ShouldBeTrue) + test.That(t, spatial.OrientationAlmostEqual(part.FrameConfig.Orientation, testPose.Orientation()), test.ShouldBeTrue) }) } diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 6759c65d4b4..593b13eb1d0 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -3,10 +3,8 @@ package motionplan import ( "errors" "math" - "strconv" "github.com/golang/geo/r3" - commonpb "go.viam.com/api/common/v1" "go.viam.com/rdk/referenceframe" spatial "go.viam.com/rdk/spatialmath" @@ -193,38 +191,13 @@ func NewCollisionConstraint( func NewCollisionConstraintFromWorldState( frame referenceframe.Frame, fs referenceframe.FrameSystem, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, observationInput map[string][]referenceframe.Input, reportDistances bool, ) (Constraint, error) { - transformGeometriesToWorldFrame := func(gfs []*commonpb.GeometriesInFrame) (*referenceframe.GeometriesInFrame, error) { - allGeometries := make(map[string]spatial.Geometry) - for name1, gf := range gfs { - obstacles, err := referenceframe.ProtobufToGeometriesInFrame(gf) - if err != nil { - return nil, err - } - // TODO(rb) it is bad practice to assume that the current inputs of the robot correspond to the passed in world state - // the state that observed the worldState should ultimately be included as part of the worldState message - tf, err := fs.Transform(observationInput, obstacles, referenceframe.World) - if err != nil { - return nil, err - } - for name2, g := range tf.(*referenceframe.GeometriesInFrame).Geometries() { - geomName := strconv.Itoa(name1) + "_" + name2 - if _, present := allGeometries[geomName]; present { - return nil, errors.New("multiple geometries with the same name") - } - allGeometries[geomName] = g - } - } - return referenceframe.NewGeometriesInFrame(referenceframe.World, allGeometries), nil - } - obstacles, err := transformGeometriesToWorldFrame(worldState.GetObstacles()) - if err != nil { - return nil, err - } - interactionSpaces, err := transformGeometriesToWorldFrame(worldState.GetInteractionSpaces()) + // TODO(rb) it is bad practice to assume that the current inputs of the robot correspond to the passed in world state + // the state that observed the worldState should ultimately be included as part of the worldState message + worldState, err := worldState.ToWorldFrame(fs, observationInput) if err != nil { return nil, err } @@ -240,7 +213,14 @@ func NewCollisionConstraintFromWorldState( if err != nil { return nil, err } - return NewCollisionConstraint(frame, goodInputs, obstacles.Geometries(), interactionSpaces.Geometries(), reportDistances), nil + + return NewCollisionConstraint( + frame, + goodInputs, + worldState.Obstacles[0].Geometries(), + worldState.InteractionSpaces[0].Geometries(), + reportDistances, + ), nil } // NewAbsoluteLinearInterpolatingConstraint provides a Constraint whose valid manifold allows a specified amount of deviation from the diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 9e8484dcf9c..a919629fa41 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -11,7 +11,6 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" - commonpb "go.viam.com/api/common/v1" "go.viam.com/utils" frame "go.viam.com/rdk/referenceframe" @@ -39,7 +38,7 @@ func PlanMotion(ctx context.Context, f frame.Frame, seedMap map[string][]frame.Input, fs frame.FrameSystem, - worldState *commonpb.WorldState, + worldState *frame.WorldState, planningOpts map[string]interface{}, ) ([]map[string][]frame.Input, error) { return PlanWaypoints(ctx, logger, []*frame.PoseInFrame{dst}, f, seedMap, fs, worldState, []map[string]interface{}{planningOpts}) @@ -51,7 +50,7 @@ func PlanRobotMotion(ctx context.Context, f frame.Frame, r robot.Robot, fs frame.FrameSystem, - worldState *commonpb.WorldState, + worldState *frame.WorldState, planningOpts map[string]interface{}, ) ([]map[string][]frame.Input, error) { seedMap, _, err := framesystem.RobotFsCurrentInputs(ctx, r, fs) @@ -103,7 +102,7 @@ func PlanWaypoints(ctx context.Context, f frame.Frame, seedMap map[string][]frame.Input, fs frame.FrameSystem, - worldState *commonpb.WorldState, + worldState *frame.WorldState, planningOpts []map[string]interface{}, ) ([]map[string][]frame.Input, error) { solvableFS := NewSolvableFrameSystem(fs, logger) @@ -394,4 +393,3 @@ func extractPath(startMap, goalMap map[node]node, pair *nodePair) []node { } return path } - diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 94729f8d9cb..134d60b64d5 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -10,7 +10,6 @@ import ( "time" "github.com/edaniels/golog" - commonpb "go.viam.com/api/common/v1" "go.viam.com/utils" "go.viam.com/rdk/referenceframe" @@ -46,7 +45,7 @@ func newPlanManager(frame *solverFrame, fs referenceframe.FrameSystem, logger go func (mp *planManager) PlanSingleWaypoint(ctx context.Context, seedMap map[string][]referenceframe.Input, goalPos spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, motionConfig map[string]interface{}, ) ([][]referenceframe.Input, error) { seed, err := mp.frame.mapToSlice(seedMap) @@ -291,7 +290,7 @@ func (mp *planManager) planMotion( func (mp *planManager) plannerSetupFromMoveRequest( from, to spatialmath.Pose, seedMap map[string][]referenceframe.Input, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, planningOpts map[string]interface{}, ) (*plannerOptions, error) { // Start with normal options diff --git a/motionplan/solvableFrameSystem.go b/motionplan/solvableFrameSystem.go index 12201e07d18..40103ff0d44 100644 --- a/motionplan/solvableFrameSystem.go +++ b/motionplan/solvableFrameSystem.go @@ -7,7 +7,6 @@ import ( "github.com/edaniels/golog" "go.uber.org/multierr" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/arm/v1" frame "go.viam.com/rdk/referenceframe" @@ -46,7 +45,7 @@ func (fss *SolvableFrameSystem) SolveWaypointsWithOptions(ctx context.Context, seedMap map[string][]frame.Input, goals []*frame.PoseInFrame, solveFrameName string, - worldState *commonpb.WorldState, + worldState *frame.WorldState, motionConfigs []map[string]interface{}, ) ([]map[string][]frame.Input, error) { steps := make([]map[string][]frame.Input, 0, len(goals)*2) diff --git a/motionplan/solvableFrameSystem_test.go b/motionplan/solvableFrameSystem_test.go index 6349ca11ede..5ec363578ea 100644 --- a/motionplan/solvableFrameSystem_test.go +++ b/motionplan/solvableFrameSystem_test.go @@ -7,7 +7,6 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" - commonpb "go.viam.com/api/common/v1" "go.viam.com/test" frame "go.viam.com/rdk/referenceframe" @@ -161,8 +160,8 @@ func TestMovementWithGripper(t *testing.T) { test.That(t, err, test.ShouldBeNil) geometries := make(map[string]spatial.Geometry) geometries["obstacle"] = obstacle - obstacles := []*commonpb.GeometriesInFrame{frame.GeometriesInFrameToProtobuf(frame.NewGeometriesInFrame(frame.World, geometries))} - worldState := &commonpb.WorldState{Obstacles: obstacles} + obstacles := frame.NewGeometriesInFrame(frame.World, geometries) + worldState := &frame.WorldState{Obstacles: []*frame.GeometriesInFrame{obstacles}} sfPlanner, err = newPlanManager(sf, solver, solver.logger, 1) test.That(t, err, test.ShouldBeNil) solution, err = sfPlanner.PlanSingleWaypoint(context.Background(), zeroPosition, goal, worldState, nil) diff --git a/referenceframe/errors.go b/referenceframe/errors.go index 756453f1a60..d3a8955e35a 100644 --- a/referenceframe/errors.go +++ b/referenceframe/errors.go @@ -2,6 +2,9 @@ package referenceframe import "github.com/pkg/errors" +// ErrEmptyStringFrameName denotes an error when a frame with a name "" is specified. +var ErrEmptyStringFrameName = errors.New("frame with name \"\" cannot be used") + // NewParentFrameMissingError returns an error indicating that the parent frame is nil. func NewParentFrameMissingError() error { return errors.New("parent frame is nil") diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index a986eee8854..6364f277c24 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -351,7 +351,7 @@ func (sfs *simpleFrameSystem) transformFromParent(inputMap map[string][]Input, s } // transform from source to world, world to target parent - return &PoseInFrame{dst.Name(), spatial.Compose(spatial.PoseInverse(dstToWorld), srcToWorld)}, nil + return NewPoseInFrame(dst.Name(), spatial.Compose(spatial.PoseInverse(dstToWorld), srcToWorld)), nil } // compose the quaternions from the input frame to the world referenceframe. diff --git a/referenceframe/transformable.go b/referenceframe/transformable.go index 6d87608cb71..4b76c0b5c00 100644 --- a/referenceframe/transformable.go +++ b/referenceframe/transformable.go @@ -3,6 +3,7 @@ package referenceframe import ( "strconv" + "github.com/pkg/errors" commonpb "go.viam.com/api/common/v1" "go.viam.com/rdk/spatialmath" @@ -19,6 +20,7 @@ type Transformable interface { type PoseInFrame struct { frame string pose spatialmath.Pose + name string } // FrameName returns the name of the frame in which the pose was observed. @@ -31,6 +33,11 @@ func (pF *PoseInFrame) Pose() spatialmath.Pose { return pF.pose } +// Name returns the name of the PoseInFrame. +func (pF *PoseInFrame) Name() string { + return pF.name +} + // Transform changes the PoseInFrame pF into the reference frame specified by the tf argument. // The tf PoseInFrame represents the pose of the pF reference frame with respect to the destination reference frame. func (pF *PoseInFrame) Transform(tf *PoseInFrame) Transformable { @@ -45,8 +52,16 @@ func NewPoseInFrame(frame string, pose spatialmath.Pose) *PoseInFrame { } } -// PoseInFrameToProtobuf converts a PoseInFrame struct to a -// PoseInFrame message as specified in common.proto. +// NewNamedPoseInFrame generates a new PoseInFrame and gives it the specified name. +func NewNamedPoseInFrame(frame string, pose spatialmath.Pose, name string) *PoseInFrame { + return &PoseInFrame{ + frame: frame, + pose: pose, + name: name, + } +} + +// PoseInFrameToProtobuf converts a PoseInFrame struct to a PoseInFrame protobuf message. func PoseInFrameToProtobuf(framedPose *PoseInFrame) *commonpb.PoseInFrame { poseProto := spatialmath.PoseToProtobuf(framedPose.pose) return &commonpb.PoseInFrame{ @@ -55,8 +70,7 @@ func PoseInFrameToProtobuf(framedPose *PoseInFrame) *commonpb.PoseInFrame { } } -// ProtobufToPoseInFrame converts a PoseInFrame message as specified in -// common.proto to a PoseInFrame struct. +// ProtobufToPoseInFrame converts a PoseInFrame protobuf message to a PoseInFrame struct. func ProtobufToPoseInFrame(proto *commonpb.PoseInFrame) *PoseInFrame { result := &PoseInFrame{} result.pose = spatialmath.NewPoseFromProtobuf(proto.GetPose()) @@ -64,6 +78,61 @@ func ProtobufToPoseInFrame(proto *commonpb.PoseInFrame) *PoseInFrame { return result } +// PoseInFrameToTransformProtobuf converts a PoseInFrame struct to a Transform protobuf message. +func PoseInFrameToTransformProtobuf(framedPose *PoseInFrame) (*commonpb.Transform, error) { + if framedPose.name == "" { + return nil, ErrEmptyStringFrameName + } + return &commonpb.Transform{ + ReferenceFrame: framedPose.name, + PoseInObserverFrame: PoseInFrameToProtobuf(framedPose), + }, nil +} + +// PoseInFrameFromTransformProtobuf converts a Transform protobuf message to a PoseInFrame struct. +func PoseInFrameFromTransformProtobuf(proto *commonpb.Transform) (*PoseInFrame, error) { + frameName := proto.GetReferenceFrame() + if frameName == "" { + return nil, ErrEmptyStringFrameName + } + poseInObserverFrame := proto.GetPoseInObserverFrame() + parentFrame := poseInObserverFrame.GetReferenceFrame() + if parentFrame == "" { + return nil, ErrEmptyStringFrameName + } + poseMsg := poseInObserverFrame.GetPose() + pose := spatialmath.NewPoseFromProtobuf(poseMsg) + return NewNamedPoseInFrame(parentFrame, pose, frameName), nil +} + +// PoseInFramesToTransformProtobuf converts a slice of PoseInFrame structs to a slice of Transform protobuf messages. +// TODO(rb): use generics to operate on lists of arbirary types. +func PoseInFramesToTransformProtobuf(poseSlice []*PoseInFrame) ([]*commonpb.Transform, error) { + protoTransforms := make([]*commonpb.Transform, 0, len(poseSlice)) + for i, transform := range poseSlice { + protoTf, err := PoseInFrameToTransformProtobuf(transform) + if err != nil { + return nil, errors.Wrapf(err, "conversion error at index %d", i) + } + protoTransforms = append(protoTransforms, protoTf) + } + return protoTransforms, nil +} + +// PoseInFramesFromTransformProtobuf converts a slice of Transform protobuf messages to a slice of PoseInFrame structs. +// TODO(rb): use generics to operate on lists of arbirary proto types. +func PoseInFramesFromTransformProtobuf(protoSlice []*commonpb.Transform) ([]*PoseInFrame, error) { + transforms := make([]*PoseInFrame, 0, len(protoSlice)) + for i, protoTransform := range protoSlice { + transform, err := PoseInFrameFromTransformProtobuf(protoTransform) + if err != nil { + return nil, errors.Wrapf(err, "conversion error at index %d", i) + } + transforms = append(transforms, transform) + } + return transforms, nil +} + // GeometriesInFrame is a data structure that packages geometries with the name of the frame in which it was observed. type GeometriesInFrame struct { frame string diff --git a/referenceframe/worldstate.go b/referenceframe/worldstate.go new file mode 100644 index 00000000000..45506ee410c --- /dev/null +++ b/referenceframe/worldstate.go @@ -0,0 +1,112 @@ +package referenceframe + +import ( + "strconv" + + "github.com/pkg/errors" + commonpb "go.viam.com/api/common/v1" + + spatial "go.viam.com/rdk/spatialmath" +) + +// WorldState is a struct to store the data representation of the robot's environment. +type WorldState struct { + Obstacles, InteractionSpaces []*GeometriesInFrame + Transforms []*PoseInFrame +} + +// WorldStateFromProtobuf takes the protobuf definition of a WorldState and converts it to a rdk defined WorldState. +func WorldStateFromProtobuf(proto *commonpb.WorldState) (*WorldState, error) { + convertProtoGeometries := func(allProtoGeometries []*commonpb.GeometriesInFrame) ([]*GeometriesInFrame, error) { + list := make([]*GeometriesInFrame, 0, len(allProtoGeometries)) + for _, protoGeometries := range allProtoGeometries { + geometries, err := ProtobufToGeometriesInFrame(protoGeometries) + if err != nil { + return nil, err + } + list = append(list, geometries) + } + return list, nil + } + if proto == nil { + return &WorldState{}, nil + } + obstacles, err := convertProtoGeometries(proto.GetObstacles()) + if err != nil { + return nil, err + } + interactionSpaces, err := convertProtoGeometries(proto.GetInteractionSpaces()) + if err != nil { + return nil, err + } + transforms, err := PoseInFramesFromTransformProtobuf(proto.GetTransforms()) + if err != nil { + return nil, err + } + + return &WorldState{ + Obstacles: obstacles, + InteractionSpaces: interactionSpaces, + Transforms: transforms, + }, nil +} + +// WorldStateToProtobuf takes an rdk WorldState and converts it to the protobuf definition of a WorldState. +func WorldStateToProtobuf(worldState *WorldState) (*commonpb.WorldState, error) { + convertGeometriesToProto := func(allGeometries []*GeometriesInFrame) []*commonpb.GeometriesInFrame { + list := make([]*commonpb.GeometriesInFrame, 0, len(allGeometries)) + for _, geometries := range allGeometries { + list = append(list, GeometriesInFrameToProtobuf(geometries)) + } + return list + } + + transforms, err := PoseInFramesToTransformProtobuf(worldState.Transforms) + if err != nil { + return nil, err + } + + return &commonpb.WorldState{ + Obstacles: convertGeometriesToProto(worldState.Obstacles), + InteractionSpaces: convertGeometriesToProto(worldState.InteractionSpaces), + Transforms: transforms, + }, nil +} + +// ToWorldFrame takes a frame system and a set of inputs for that frame system and converts all the geometries +// in the WorldState such that they are in the frame system's World reference frame. +func (ws *WorldState) ToWorldFrame(fs FrameSystem, inputs map[string][]Input) (*WorldState, error) { + transformGeometriesToWorldFrame := func(gfs []*GeometriesInFrame) (*GeometriesInFrame, error) { + allGeometries := make(map[string]spatial.Geometry) + for name1, gf := range gfs { + tf, err := fs.Transform(inputs, gf, World) + if err != nil { + return nil, err + } + for name2, g := range tf.(*GeometriesInFrame).Geometries() { + geomName := strconv.Itoa(name1) + "_" + name2 + if _, present := allGeometries[geomName]; present { + return nil, errors.New("multiple geometries with the same name") + } + allGeometries[geomName] = g + } + } + return NewGeometriesInFrame(World, allGeometries), nil + } + + if ws == nil { + ws = &WorldState{} + } + obstacles, err := transformGeometriesToWorldFrame(ws.Obstacles) + if err != nil { + return nil, err + } + interactionSpaces, err := transformGeometriesToWorldFrame(ws.InteractionSpaces) + if err != nil { + return nil, err + } + return &WorldState{ + Obstacles: []*GeometriesInFrame{obstacles}, + InteractionSpaces: []*GeometriesInFrame{interactionSpaces}, + }, nil +} diff --git a/robot/client/client.go b/robot/client/client.go index 33cb4dda8b0..1e75314bfab 100644 --- a/robot/client/client.go +++ b/robot/client/client.go @@ -721,10 +721,15 @@ func (rc *RobotClient) DiscoverComponents(ctx context.Context, qs []discovery.Qu } // FrameSystemConfig returns the info of each individual part that makes up the frame system. -func (rc *RobotClient) FrameSystemConfig(ctx context.Context, additionalTransforms []*commonpb.Transform) (framesystemparts.Parts, error) { - resp, err := rc.client.FrameSystemConfig(ctx, &pb.FrameSystemConfigRequest{ - SupplementalTransforms: additionalTransforms, - }) +func (rc *RobotClient) FrameSystemConfig( + ctx context.Context, + additionalTransforms []*referenceframe.PoseInFrame, +) (framesystemparts.Parts, error) { + transforms, err := referenceframe.PoseInFramesToTransformProtobuf(additionalTransforms) + if err != nil { + return nil, err + } + resp, err := rc.client.FrameSystemConfig(ctx, &pb.FrameSystemConfigRequest{SupplementalTransforms: transforms}) if err != nil { return nil, err } @@ -745,12 +750,16 @@ func (rc *RobotClient) TransformPose( ctx context.Context, query *referenceframe.PoseInFrame, destination string, - additionalTransforms []*commonpb.Transform, + additionalTransforms []*referenceframe.PoseInFrame, ) (*referenceframe.PoseInFrame, error) { + transforms, err := referenceframe.PoseInFramesToTransformProtobuf(additionalTransforms) + if err != nil { + return nil, err + } resp, err := rc.client.TransformPose(ctx, &pb.TransformPoseRequest{ Destination: destination, Source: referenceframe.PoseInFrameToProtobuf(query), - SupplementalTransforms: additionalTransforms, + SupplementalTransforms: transforms, }) if err != nil { return nil, err diff --git a/robot/client/client_test.go b/robot/client/client_test.go index 63810bd24bf..7e6a7c01920 100644 --- a/robot/client/client_test.go +++ b/robot/client/client_test.go @@ -302,7 +302,7 @@ func TestStatusClient(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "no arm") - err = arm1.MoveToPosition(context.Background(), spatialmath.NewPoseFromPoint(r3.Vector{X: 1}), &commonpb.WorldState{}, nil) + err = arm1.MoveToPosition(context.Background(), spatialmath.NewPoseFromPoint(r3.Vector{X: 1}), &referenceframe.WorldState{}, nil) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "no arm") @@ -371,7 +371,11 @@ func TestStatusClient(t *testing.T) { test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "no arm") - err = resource1.(arm.Arm).MoveToPosition(context.Background(), spatialmath.NewPoseFromPoint(r3.Vector{X: 1}), &commonpb.WorldState{}, nil) + err = resource1.(arm.Arm).MoveToPosition( + context.Background(), + spatialmath.NewPoseFromPoint(r3.Vector{X: 1}), + &referenceframe.WorldState{}, nil, + ) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, "no arm") @@ -1259,14 +1263,14 @@ func TestClientConfig(t *testing.T) { workingRobot.FrameSystemConfigFunc = func( ctx context.Context, - additionalTransforms []*commonpb.Transform, + additionalTransforms []*referenceframe.PoseInFrame, ) (framesystemparts.Parts, error) { return framesystemparts.Parts(fsConfigs), nil } configErr := errors.New("failed to retrieve config") failingRobot.FrameSystemConfigFunc = func( ctx context.Context, - additionalTransforms []*commonpb.Transform, + additionalTransforms []*referenceframe.PoseInFrame, ) (framesystemparts.Parts, error) { return nil, configErr } diff --git a/robot/framesystem/framesystem.go b/robot/framesystem/framesystem.go index 01f81ec603b..ec874b84a9b 100644 --- a/robot/framesystem/framesystem.go +++ b/robot/framesystem/framesystem.go @@ -8,7 +8,6 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" "go.opencensus.io/trace" - commonpb "go.viam.com/api/common/v1" "go.viam.com/rdk/config" "go.viam.com/rdk/referenceframe" @@ -25,10 +24,10 @@ const LocalFrameSystemName = "robot" // A Service that returns the frame system for a robot. type Service interface { - Config(ctx context.Context, additionalTransforms []*commonpb.Transform) (framesystemparts.Parts, error) + Config(ctx context.Context, additionalTransforms []*referenceframe.PoseInFrame) (framesystemparts.Parts, error) TransformPose( ctx context.Context, pose *referenceframe.PoseInFrame, dst string, - additionalTransforms []*commonpb.Transform, + additionalTransforms []*referenceframe.PoseInFrame, ) (*referenceframe.PoseInFrame, error) } @@ -122,7 +121,10 @@ func (svc *frameSystemService) Update(ctx context.Context, resources map[resourc // The output of this function is to be sent over GRPC to the client, so the client // can build its frame system. requests the remote components from the remote's frame system service. // NOTE(RDK-258): If remotes can trigger a local robot to reconfigure, you don't need to update remotes in every call. -func (svc *frameSystemService) Config(ctx context.Context, additionalTransforms []*commonpb.Transform) (framesystemparts.Parts, error) { +func (svc *frameSystemService) Config( + ctx context.Context, + additionalTransforms []*referenceframe.PoseInFrame, +) (framesystemparts.Parts, error) { ctx, span := trace.StartSpan(ctx, "services::framesystem::Config") defer span.End() // update parts from remotes @@ -133,7 +135,7 @@ func (svc *frameSystemService) Config(ctx context.Context, additionalTransforms // build the config allParts := combineParts(svc.localParts, svc.offsetParts, remoteParts) for _, transformMsg := range additionalTransforms { - newPart, err := config.ConvertTransformProtobufToFrameSystemPart(transformMsg) + newPart, err := config.PoseInFrameToFrameSystemPart(transformMsg) if err != nil { return nil, err } @@ -151,7 +153,7 @@ func (svc *frameSystemService) TransformPose( ctx context.Context, pose *referenceframe.PoseInFrame, dst string, - additionalTransforms []*commonpb.Transform, + additionalTransforms []*referenceframe.PoseInFrame, ) (*referenceframe.PoseInFrame, error) { ctx, span := trace.StartSpan(ctx, "services::framesystem::TransformPose") defer span.End() diff --git a/robot/framesystem/framesystem_utils.go b/robot/framesystem/framesystem_utils.go index 36425b37e08..9bc4515d05b 100644 --- a/robot/framesystem/framesystem_utils.go +++ b/robot/framesystem/framesystem_utils.go @@ -8,7 +8,6 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" "go.opencensus.io/trace" - commonpb "go.viam.com/api/common/v1" "go.viam.com/rdk/config" "go.viam.com/rdk/referenceframe" @@ -22,7 +21,7 @@ import ( func RobotFrameSystem( ctx context.Context, r robot.Robot, - additionalTransforms []*commonpb.Transform, + additionalTransforms []*referenceframe.PoseInFrame, ) (referenceframe.FrameSystem, error) { ctx, span := trace.StartSpan(ctx, "services::framesystem::RobotFrameSystem") defer span.End() diff --git a/robot/framesystem/service_test.go b/robot/framesystem/service_test.go index 754772f2c3c..54e5f8ff602 100644 --- a/robot/framesystem/service_test.go +++ b/robot/framesystem/service_test.go @@ -8,8 +8,6 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" "github.com/pkg/errors" - // register. - commonpb "go.viam.com/api/common/v1" "go.viam.com/test" "go.viam.com/rdk/components/base" @@ -48,44 +46,15 @@ func TestFrameSystemFromConfig(t *testing.T) { &spatialmath.R4AA{Theta: math.Pi / 2, RX: 0., RY: 1., RZ: 0.}, ) - transformMsgs := []*commonpb.Transform{ - { - ReferenceFrame: "frame1", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "pieceArm", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - { - ReferenceFrame: "frame2", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "pieceGripper", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - { - ReferenceFrame: "frame2a", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "frame2", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - { - ReferenceFrame: "frame2c", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "frame2", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - { - ReferenceFrame: "frame3", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "world", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, + transforms := []*referenceframe.PoseInFrame{ + referenceframe.NewNamedPoseInFrame("pieceArm", testPose, "frame1"), + referenceframe.NewNamedPoseInFrame("pieceGripper", testPose, "frame2"), + referenceframe.NewNamedPoseInFrame("frame2", testPose, "frame2a"), + referenceframe.NewNamedPoseInFrame("frame2", testPose, "frame2c"), + referenceframe.NewNamedPoseInFrame(referenceframe.World, testPose, "frame3"), } - fs, err := framesystem.RobotFrameSystem(context.Background(), r, transformMsgs) + + fs, err := framesystem.RobotFrameSystem(context.Background(), r, transforms) test.That(t, err, test.ShouldBeNil) // 4 frames defined + 5 from transforms, 18 frames when including the offset, test.That(t, len(fs.FrameNames()), test.ShouldEqual, 18) @@ -236,22 +205,6 @@ func TestWrongFrameSystems(t *testing.T) { &spatialmath.R4AA{Theta: math.Pi / 2, RX: 0., RY: 1., RZ: 0.}, ) - transformMsgs := []*commonpb.Transform{ - { - ReferenceFrame: "frame1", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "pieceArm", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - { - ReferenceFrame: "frame2", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "noParent", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - } cfg, err = config.Read(context.Background(), rdkutils.ResolveFile("robot/impl/data/fake.json"), logger) test.That(t, err, test.ShouldBeNil) @@ -259,20 +212,17 @@ func TestWrongFrameSystems(t *testing.T) { test.That(t, err, test.ShouldBeNil) defer r.Close(context.Background()) - fs, err := framesystem.RobotFrameSystem(context.Background(), r, transformMsgs) + transforms := []*referenceframe.PoseInFrame{ + referenceframe.NewNamedPoseInFrame("pieceArm", testPose, "frame1"), + referenceframe.NewNamedPoseInFrame("noParent", testPose, "frame2"), + } + fs, err := framesystem.RobotFrameSystem(context.Background(), r, transforms) test.That(t, err, test.ShouldBeError, framesystemparts.NewMissingParentError("frame2", "noParent")) test.That(t, fs, test.ShouldBeNil) - transformMsgs = []*commonpb.Transform{ - { - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "pieceArm", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - } - fs, err = framesystem.RobotFrameSystem(context.Background(), r, transformMsgs) - test.That(t, err, test.ShouldBeError, config.NewMissingReferenceFrameError(&commonpb.Transform{})) + transforms = []*referenceframe.PoseInFrame{referenceframe.NewPoseInFrame("pieceArm", testPose)} + fs, err = framesystem.RobotFrameSystem(context.Background(), r, transforms) + test.That(t, err, test.ShouldBeError, referenceframe.ErrEmptyStringFrameName) test.That(t, fs, test.ShouldBeNil) } @@ -346,51 +296,21 @@ func TestServiceWithRemote(t *testing.T) { &spatialmath.R4AA{Theta: math.Pi / 2, RX: 0., RY: 1., RZ: 0.}, ) - transformMsgs := []*commonpb.Transform{ - { - ReferenceFrame: "frame1", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "bar:pieceArm", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - { - ReferenceFrame: "frame2", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "bar:pieceGripper", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - { - ReferenceFrame: "frame2a", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "frame2", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - { - ReferenceFrame: "frame2c", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "frame2", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - { - ReferenceFrame: "frame3", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "world", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, + transforms := []*referenceframe.PoseInFrame{ + referenceframe.NewNamedPoseInFrame("bar:pieceArm", testPose, "frame1"), + referenceframe.NewNamedPoseInFrame("bar:pieceGripper", testPose, "frame2"), + referenceframe.NewNamedPoseInFrame("frame2", testPose, "frame2a"), + referenceframe.NewNamedPoseInFrame("frame2", testPose, "frame2c"), + referenceframe.NewNamedPoseInFrame(referenceframe.World, testPose, "frame3"), } r2, err := robotimpl.New(context.Background(), localConfig, logger) test.That(t, err, test.ShouldBeNil) - fs, err := framesystem.RobotFrameSystem(context.Background(), r2, transformMsgs) + fs, err := framesystem.RobotFrameSystem(context.Background(), r2, transforms) test.That(t, err, test.ShouldBeNil) test.That(t, fs.FrameNames(), test.ShouldHaveLength, 34) // run the frame system service - allParts, err := r2.FrameSystemConfig(context.Background(), transformMsgs) + allParts, err := r2.FrameSystemConfig(context.Background(), transforms) test.That(t, err, test.ShouldBeNil) t.Logf("frame system:\n%v", allParts) } diff --git a/robot/impl/local_robot.go b/robot/impl/local_robot.go index 3bf2099ed82..c81d8979d2e 100644 --- a/robot/impl/local_robot.go +++ b/robot/impl/local_robot.go @@ -13,7 +13,6 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" "go.uber.org/multierr" - commonpb "go.viam.com/api/common/v1" goutils "go.viam.com/utils" "go.viam.com/utils/pexec" "go.viam.com/utils/rpc" @@ -614,7 +613,10 @@ func (r *localRobot) Refresh(ctx context.Context) error { } // FrameSystemConfig returns the info of each individual part that makes up a robot's frame system. -func (r *localRobot) FrameSystemConfig(ctx context.Context, additionalTransforms []*commonpb.Transform) (framesystemparts.Parts, error) { +func (r *localRobot) FrameSystemConfig( + ctx context.Context, + additionalTransforms []*referenceframe.PoseInFrame, +) (framesystemparts.Parts, error) { framesystem, err := r.fsService() if err != nil { return nil, err @@ -628,7 +630,7 @@ func (r *localRobot) TransformPose( ctx context.Context, pose *referenceframe.PoseInFrame, dst string, - additionalTransforms []*commonpb.Transform, + additionalTransforms []*referenceframe.PoseInFrame, ) (*referenceframe.PoseInFrame, error) { framesystem, err := r.fsService() if err != nil { diff --git a/robot/impl/local_robot_test.go b/robot/impl/local_robot_test.go index c3c608c2d89..d0f3cbe264b 100644 --- a/robot/impl/local_robot_test.go +++ b/robot/impl/local_robot_test.go @@ -720,7 +720,7 @@ func (da *dummyArm) EndPosition(ctx context.Context, extra map[string]interface{ func (da *dummyArm) MoveToPosition( ctx context.Context, pose spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { return nil diff --git a/robot/impl/resource_manager_test.go b/robot/impl/resource_manager_test.go index 26195b002b7..c7a31641c09 100644 --- a/robot/impl/resource_manager_test.go +++ b/robot/impl/resource_manager_test.go @@ -13,7 +13,6 @@ import ( "github.com/jhump/protoreflect/desc" "github.com/jhump/protoreflect/grpcreflect" "go.mongodb.org/mongo-driver/bson/primitive" - commonpb "go.viam.com/api/common/v1" armpb "go.viam.com/api/component/arm/v1" basepb "go.viam.com/api/component/base/v1" boardpb "go.viam.com/api/component/board/v1" @@ -492,7 +491,7 @@ func TestManagerAdd(t *testing.T) { ctx context.Context, componentName resource.Name, grabPose *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { return false, nil @@ -1799,7 +1798,7 @@ func (rr *dummyRobot) ResourceByName(name resource.Name) (interface{}, error) { // FrameSystemConfig returns a remote robot's FrameSystem Config. func (rr *dummyRobot) FrameSystemConfig( ctx context.Context, - additionalTransforms []*commonpb.Transform, + additionalTransforms []*referenceframe.PoseInFrame, ) (framesystemparts.Parts, error) { panic("change to return nil") } @@ -1808,7 +1807,7 @@ func (rr *dummyRobot) TransformPose( ctx context.Context, pose *referenceframe.PoseInFrame, dst string, - additionalTransforms []*commonpb.Transform, + additionalTransforms []*referenceframe.PoseInFrame, ) (*referenceframe.PoseInFrame, error) { panic("change to return nil") } diff --git a/robot/robot.go b/robot/robot.go index b9959b9184c..ff185c64ee1 100644 --- a/robot/robot.go +++ b/robot/robot.go @@ -5,7 +5,6 @@ import ( "context" "github.com/edaniels/golog" - commonpb "go.viam.com/api/common/v1" "go.viam.com/utils/pexec" "go.viam.com/rdk/config" @@ -54,14 +53,14 @@ type Robot interface { Logger() golog.Logger // FrameSystemConfig returns the individual parts that make up a robot's frame system - FrameSystemConfig(ctx context.Context, additionalTransforms []*commonpb.Transform) (framesystemparts.Parts, error) + FrameSystemConfig(ctx context.Context, additionalTransforms []*referenceframe.PoseInFrame) (framesystemparts.Parts, error) // TransformPose will transform the pose of the requested poseInFrame to the desired frame in the robot's frame system. TransformPose( ctx context.Context, pose *referenceframe.PoseInFrame, dst string, - additionalTransforms []*commonpb.Transform, + additionalTransforms []*referenceframe.PoseInFrame, ) (*referenceframe.PoseInFrame, error) // Status takes a list of resource names and returns their corresponding statuses. If no names are passed in, return all statuses. diff --git a/robot/server/server.go b/robot/server/server.go index 399302060ef..546d5f18be7 100644 --- a/robot/server/server.go +++ b/robot/server/server.go @@ -174,7 +174,11 @@ func (s *Server) FrameSystemConfig( ctx context.Context, req *pb.FrameSystemConfigRequest, ) (*pb.FrameSystemConfigResponse, error) { - sortedParts, err := s.r.FrameSystemConfig(ctx, req.GetSupplementalTransforms()) + transforms, err := referenceframe.PoseInFramesFromTransformProtobuf(req.GetSupplementalTransforms()) + if err != nil { + return nil, err + } + sortedParts, err := s.r.FrameSystemConfig(ctx, transforms) if err != nil { return nil, err } @@ -195,10 +199,11 @@ func (s *Server) FrameSystemConfig( // TransformPose will transform the pose of the requested poseInFrame to the desired frame in the robot's frame system. func (s *Server) TransformPose(ctx context.Context, req *pb.TransformPoseRequest) (*pb.TransformPoseResponse, error) { - dst := req.Destination - pF := referenceframe.ProtobufToPoseInFrame(req.Source) - transformedPose, err := s.r.TransformPose(ctx, pF, dst, req.GetSupplementalTransforms()) - + transforms, err := referenceframe.PoseInFramesFromTransformProtobuf(req.GetSupplementalTransforms()) + if err != nil { + return nil, err + } + transformedPose, err := s.r.TransformPose(ctx, referenceframe.ProtobufToPoseInFrame(req.Source), req.Destination, transforms) return &pb.TransformPoseResponse{Pose: referenceframe.PoseInFrameToProtobuf(transformedPose)}, err } diff --git a/robot/server/server_test.go b/robot/server/server_test.go index 2959f4d34c8..b76c37df81d 100644 --- a/robot/server/server_test.go +++ b/robot/server/server_test.go @@ -177,7 +177,7 @@ func TestServerFrameSystemConfig(t *testing.T) { } injectRobot.FrameSystemConfigFunc = func( - ctx context.Context, additionalTransforms []*commonpb.Transform, + ctx context.Context, additionalTransforms []*referenceframe.PoseInFrame, ) (framesystemparts.Parts, error) { return framesystemparts.Parts(fsConfigs), nil } @@ -228,7 +228,7 @@ func TestServerFrameSystemConfig(t *testing.T) { t.Run("test failing config function", func(t *testing.T) { expectedErr := errors.New("failed to retrieve config") injectRobot.FrameSystemConfigFunc = func( - ctx context.Context, additionalTransforms []*commonpb.Transform, + ctx context.Context, additionalTransforms []*referenceframe.PoseInFrame, ) (framesystemparts.Parts, error) { return nil, expectedErr } diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 14975338683..7fc780bb30e 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -8,7 +8,6 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" - commonpb "go.viam.com/api/common/v1" "go.viam.com/rdk/components/arm" "go.viam.com/rdk/config" "go.viam.com/rdk/motionplan" @@ -49,7 +48,7 @@ func (ms *builtIn) Move( ctx context.Context, componentName resource.Name, destination *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { operation.CancelOtherWithLabel(ctx, "motion-service") @@ -59,7 +58,7 @@ func (ms *builtIn) Move( goalFrameName := destination.FrameName() logger.Debugf("goal given in frame of %q", goalFrameName) - frameSys, err := framesystem.RobotFrameSystem(ctx, ms.r, worldState.GetTransforms()) + frameSys, err := framesystem.RobotFrameSystem(ctx, ms.r, worldState.Transforms) if err != nil { return false, err } @@ -69,9 +68,9 @@ func (ms *builtIn) Move( if err != nil { return false, err } - + movingFrame := frameSys.Frame(componentName.ShortName()) - + logger.Debugf("frame system inputs: %v", fsInputs) if movingFrame == nil { return false, fmt.Errorf("component named %s not found in robot frame system", componentName.ShortName()) @@ -123,7 +122,7 @@ func (ms *builtIn) MoveSingleComponent( ctx context.Context, componentName resource.Name, destination *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { operation.CancelOtherWithLabel(ctx, "motion-service") @@ -143,7 +142,7 @@ func (ms *builtIn) MoveSingleComponent( if destination.FrameName() != componentName.ShortName() { logger.Debugf("goal given in frame of %q", destination.FrameName()) - frameSys, err := framesystem.RobotFrameSystem(ctx, ms.r, worldState.GetTransforms()) + frameSys, err := framesystem.RobotFrameSystem(ctx, ms.r, worldState.Transforms) if err != nil { return false, err } @@ -175,7 +174,7 @@ func (ms *builtIn) GetPose( ctx context.Context, componentName resource.Name, destinationFrame string, - supplementalTransforms []*commonpb.Transform, + supplementalTransforms []*referenceframe.PoseInFrame, extra map[string]interface{}, ) (*referenceframe.PoseInFrame, error) { if destinationFrame == "" { diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index f39f376ef64..b30430ffaf6 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -44,7 +44,7 @@ func TestMoveFailures(t *testing.T) { ms := setupMotionServiceFromConfig(t, "../data/arm_gantry.json") t.Run("fail on not finding gripper", func(t *testing.T) { grabPose := referenceframe.NewPoseInFrame("fakeCamera", spatialmath.NewPoseFromPoint(r3.Vector{10.0, 10.0, 10.0})) - _, err = ms.Move(context.Background(), camera.Named("fake"), grabPose, &commonpb.WorldState{}, map[string]interface{}{}) + _, err = ms.Move(context.Background(), camera.Named("fake"), grabPose, &referenceframe.WorldState{}, map[string]interface{}{}) test.That(t, err, test.ShouldNotBeNil) }) @@ -53,18 +53,8 @@ func TestMoveFailures(t *testing.T) { r3.Vector{X: 1., Y: 2., Z: 3.}, &spatialmath.R4AA{Theta: math.Pi / 2, RX: 0., RY: 1., RZ: 0.}, ) - transformMsgs := []*commonpb.Transform{ - { - ReferenceFrame: "frame2", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "noParent", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - } - worldState := &commonpb.WorldState{ - Transforms: transformMsgs, - } + transforms := []*referenceframe.PoseInFrame{referenceframe.NewNamedPoseInFrame("noParent", testPose, "frame2")} + worldState := &referenceframe.WorldState{Transforms: transforms} poseInFrame := referenceframe.NewPoseInFrame("frame2", spatialmath.NewZeroPose()) _, err = ms.Move(context.Background(), arm.Named("arm1"), poseInFrame, worldState, map[string]interface{}{}) test.That(t, err, test.ShouldBeError, framesystemparts.NewMissingParentError("frame2", "noParent")) @@ -77,19 +67,19 @@ func TestMove1(t *testing.T) { t.Run("succeeds when all frame info in config", func(t *testing.T) { grabPose := referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{0, -30, -50})) - _, err = ms.Move(context.Background(), gripper.Named("pieceGripper"), grabPose, &commonpb.WorldState{}, map[string]interface{}{}) + _, err = ms.Move(context.Background(), gripper.Named("pieceGripper"), grabPose, &referenceframe.WorldState{}, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) }) t.Run("succeeds when mobile component can be solved for destinations in own frame", func(t *testing.T) { grabPose := referenceframe.NewPoseInFrame("pieceArm", spatialmath.NewPoseFromPoint(r3.Vector{0, -30, -50})) - _, err = ms.Move(context.Background(), gripper.Named("pieceArm"), grabPose, &commonpb.WorldState{}, map[string]interface{}{}) + _, err = ms.Move(context.Background(), gripper.Named("pieceArm"), grabPose, &referenceframe.WorldState{}, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) }) t.Run("succeeds when immobile component can be solved for destinations in own frame", func(t *testing.T) { grabPose := referenceframe.NewPoseInFrame("pieceGripper", spatialmath.NewPoseFromPoint(r3.Vector{0, -30, -50})) - _, err = ms.Move(context.Background(), gripper.Named("pieceGripper"), grabPose, &commonpb.WorldState{}, map[string]interface{}{}) + _, err = ms.Move(context.Background(), gripper.Named("pieceGripper"), grabPose, &referenceframe.WorldState{}, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) }) @@ -99,25 +89,12 @@ func TestMove1(t *testing.T) { &spatialmath.R4AA{Theta: math.Pi / 2, RX: 0., RY: 1., RZ: 0.}, ) - transformMsgs := []*commonpb.Transform{ - { - ReferenceFrame: "testFrame", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "pieceArm", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - { - ReferenceFrame: "testFrame2", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "world", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - } - worldState := &commonpb.WorldState{ - Transforms: transformMsgs, + transforms := []*referenceframe.PoseInFrame{ + referenceframe.NewNamedPoseInFrame(referenceframe.World, testPose, "testFrame2"), + referenceframe.NewNamedPoseInFrame("pieceArm", testPose, "testFrame"), } + + worldState := &referenceframe.WorldState{Transforms: transforms} grabPose := referenceframe.NewPoseInFrame("testFrame2", spatialmath.NewPoseFromPoint(r3.Vector{-20, -130, -40})) _, err = ms.Move(context.Background(), gripper.Named("pieceGripper"), grabPose, worldState, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) @@ -125,7 +102,6 @@ func TestMove1(t *testing.T) { } func TestMoveWithObstacles(t *testing.T) { - var err error ms := setupMotionServiceFromConfig(t, "../data/moving_arm.json") t.Run("check a movement that should not succeed due to obstacles", func(t *testing.T) { @@ -165,14 +141,9 @@ func TestMoveWithObstacles(t *testing.T) { }, }, } - _ = obsMsgs - _, err = ms.Move( - context.Background(), - gripper.Named("pieceArm"), - grabPose, - &commonpb.WorldState{Obstacles: obsMsgs}, - map[string]interface{}{}, - ) + worldState, err := referenceframe.WorldStateFromProtobuf(&commonpb.WorldState{Obstacles: obsMsgs}) + test.That(t, err, test.ShouldBeNil) + _, err = ms.Move(context.Background(), gripper.Named("pieceArm"), grabPose, worldState, map[string]interface{}{}) // This fails due to a large obstacle being in the way test.That(t, err, test.ShouldNotBeNil) }) @@ -188,7 +159,7 @@ func TestMoveSingleComponent(t *testing.T) { context.Background(), arm.Named("pieceArm"), grabPose, - &commonpb.WorldState{}, + &referenceframe.WorldState{}, map[string]interface{}{}, ) // Gripper is not an arm and cannot move @@ -200,7 +171,7 @@ func TestMoveSingleComponent(t *testing.T) { context.Background(), gripper.Named("pieceGripper"), grabPose, - &commonpb.WorldState{}, + &referenceframe.WorldState{}, map[string]interface{}{}, ) // Gripper is not an arm and cannot move @@ -214,19 +185,8 @@ func TestMoveSingleComponent(t *testing.T) { r3.Vector{X: 1., Y: 2., Z: 3.}, &spatialmath.R4AA{Theta: math.Pi / 2, RX: 0., RY: 1., RZ: 0.}, ) - - transformMsgs := []*commonpb.Transform{ - { - ReferenceFrame: "testFrame2", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "world", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - } - worldState := &commonpb.WorldState{ - Transforms: transformMsgs, - } + transforms := []*referenceframe.PoseInFrame{referenceframe.NewNamedPoseInFrame(referenceframe.World, testPose, "testFrame2")} + worldState := &referenceframe.WorldState{Transforms: transforms} poseToGrab := spatialmath.NewPoseFromOrientation( r3.Vector{X: -20., Y: 0., Z: -800.}, @@ -243,7 +203,7 @@ func TestMultiplePieces(t *testing.T) { var err error ms := setupMotionServiceFromConfig(t, "../data/fake_tomato.json") grabPose := referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{-0, -30, -50})) - _, err = ms.Move(context.Background(), gripper.Named("gr"), grabPose, &commonpb.WorldState{}, map[string]interface{}{}) + _, err = ms.Move(context.Background(), gripper.Named("gr"), grabPose, &referenceframe.WorldState{}, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) } @@ -290,23 +250,12 @@ func TestGetPose(t *testing.T) { r3.Vector{X: 0., Y: 0., Z: 0.}, &spatialmath.R4AA{Theta: math.Pi / 2, RX: 0., RY: 1., RZ: 0.}, ) - transformMsgs := []*commonpb.Transform{ - { - ReferenceFrame: "testFrame", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "world", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - { - ReferenceFrame: "testFrame2", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "testFrame", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, + transforms := []*referenceframe.PoseInFrame{ + referenceframe.NewNamedPoseInFrame(referenceframe.World, testPose, "testFrame"), + referenceframe.NewNamedPoseInFrame("testFrame", testPose, "testFrame2"), } - pose, err = ms.GetPose(context.Background(), arm.Named("arm1"), "testFrame2", transformMsgs, map[string]interface{}{}) + + pose, err = ms.GetPose(context.Background(), arm.Named("arm1"), "testFrame2", transforms, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, -501.2) test.That(t, pose.Pose().Point().Y, test.ShouldAlmostEqual, 0) @@ -316,16 +265,8 @@ func TestGetPose(t *testing.T) { test.That(t, pose.Pose().Orientation().AxisAngles().RZ, test.ShouldEqual, 0) test.That(t, pose.Pose().Orientation().AxisAngles().Theta, test.ShouldAlmostEqual, math.Pi) - transformMsgs = []*commonpb.Transform{ - { - ReferenceFrame: "testFrame", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "noParent", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - } - pose, err = ms.GetPose(context.Background(), arm.Named("arm1"), "testFrame", transformMsgs, map[string]interface{}{}) + transforms = []*referenceframe.PoseInFrame{referenceframe.NewNamedPoseInFrame("noParent", testPose, "testFrame")} + pose, err = ms.GetPose(context.Background(), arm.Named("arm1"), "testFrame", transforms, map[string]interface{}{}) test.That(t, err, test.ShouldBeError, framesystemparts.NewMissingParentError("testFrame", "noParent")) test.That(t, pose, test.ShouldBeNil) } diff --git a/services/motion/client.go b/services/motion/client.go index 47f78254e9a..795380e1683 100644 --- a/services/motion/client.go +++ b/services/motion/client.go @@ -5,7 +5,6 @@ import ( "context" "github.com/edaniels/golog" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/service/motion/v1" vprotoutils "go.viam.com/utils/protoutils" "go.viam.com/utils/rpc" @@ -39,18 +38,22 @@ func (c *client) Move( ctx context.Context, componentName resource.Name, destination *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { ext, err := vprotoutils.StructToStructPb(extra) if err != nil { return false, err } + worldStateMsg, err := referenceframe.WorldStateToProtobuf(worldState) + if err != nil { + return false, err + } resp, err := c.client.Move(ctx, &pb.MoveRequest{ Name: c.name, ComponentName: protoutils.ResourceNameToProto(componentName), Destination: referenceframe.PoseInFrameToProtobuf(destination), - WorldState: worldState, + WorldState: worldStateMsg, Extra: ext, }) if err != nil { @@ -63,18 +66,22 @@ func (c *client) MoveSingleComponent( ctx context.Context, componentName resource.Name, destination *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { ext, err := vprotoutils.StructToStructPb(extra) if err != nil { return false, err } + worldStateMsg, err := referenceframe.WorldStateToProtobuf(worldState) + if err != nil { + return false, err + } resp, err := c.client.MoveSingleComponent(ctx, &pb.MoveSingleComponentRequest{ Name: c.name, ComponentName: protoutils.ResourceNameToProto(componentName), Destination: referenceframe.PoseInFrameToProtobuf(destination), - WorldState: worldState, + WorldState: worldStateMsg, Extra: ext, }) if err != nil { @@ -87,18 +94,22 @@ func (c *client) GetPose( ctx context.Context, componentName resource.Name, destinationFrame string, - supplementalTransforms []*commonpb.Transform, + supplementalTransforms []*referenceframe.PoseInFrame, extra map[string]interface{}, ) (*referenceframe.PoseInFrame, error) { ext, err := vprotoutils.StructToStructPb(extra) if err != nil { return nil, err } + transforms, err := referenceframe.PoseInFramesToTransformProtobuf(supplementalTransforms) + if err != nil { + return nil, err + } resp, err := c.client.GetPose(ctx, &pb.GetPoseRequest{ Name: c.name, ComponentName: protoutils.ResourceNameToProto(componentName), DestinationFrame: destinationFrame, - SupplementalTransforms: supplementalTransforms, + SupplementalTransforms: transforms, Extra: ext, }) if err != nil { diff --git a/services/motion/client_test.go b/services/motion/client_test.go index a121a0cbe15..42874ac056e 100644 --- a/services/motion/client_test.go +++ b/services/motion/client_test.go @@ -9,7 +9,6 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" "github.com/pkg/errors" - commonpb "go.viam.com/api/common/v1" servicepb "go.viam.com/api/service/motion/v1" "go.viam.com/test" "go.viam.com/utils" @@ -68,13 +67,13 @@ func TestClient(t *testing.T) { client := motion.NewClientFromConn(context.Background(), conn, testMotionServiceName, logger) - receivedTransforms := make(map[string]*commonpb.Transform) + receivedTransforms := make(map[string]*referenceframe.PoseInFrame) success := true injectMS.MoveFunc = func( ctx context.Context, componentName resource.Name, destination *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { return success, nil @@ -83,20 +82,17 @@ func TestClient(t *testing.T) { ctx context.Context, componentName resource.Name, destinationFrame string, - supplementalTransforms []*commonpb.Transform, + supplementalTransforms []*referenceframe.PoseInFrame, extra map[string]interface{}, ) (*referenceframe.PoseInFrame, error) { - for _, msg := range supplementalTransforms { - receivedTransforms[msg.GetReferenceFrame()] = msg + for _, tf := range supplementalTransforms { + receivedTransforms[tf.Name()] = tf } return referenceframe.NewPoseInFrame( destinationFrame+componentName.Name, spatialmath.NewPoseFromPoint(r3.Vector{1, 2, 3})), nil } - result, err := client.Move( - context.Background(), resourceName, grabPose, - &commonpb.WorldState{}, map[string]interface{}{}, - ) + result, err := client.Move(context.Background(), resourceName, grabPose, &referenceframe.WorldState{}, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) test.That(t, result, test.ShouldEqual, success) @@ -104,47 +100,27 @@ func TestClient(t *testing.T) { r3.Vector{X: 1., Y: 2., Z: 3.}, &spatialmath.R4AA{Theta: math.Pi / 2, RX: 0., RY: 1., RZ: 0.}, ) - transformMsgs := []*commonpb.Transform{ - { - ReferenceFrame: "frame1", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "arm1", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, - { - ReferenceFrame: "frame2", - PoseInObserverFrame: &commonpb.PoseInFrame{ - ReferenceFrame: "frame1", - Pose: spatialmath.PoseToProtobuf(testPose), - }, - }, + + transforms := []*referenceframe.PoseInFrame{ + referenceframe.NewNamedPoseInFrame("arm1", testPose, "frame1"), + referenceframe.NewNamedPoseInFrame("frame1", testPose, "frame2"), } - msgMap := make(map[string]*commonpb.Transform) - for _, msg := range transformMsgs { - msgMap[msg.GetReferenceFrame()] = msg + + tfMap := make(map[string]*referenceframe.PoseInFrame) + for _, tf := range transforms { + tfMap[tf.Name()] = tf } - poseResult, err := client.GetPose(context.Background(), arm.Named("arm1"), "foo", transformMsgs, map[string]interface{}{}) + poseResult, err := client.GetPose(context.Background(), arm.Named("arm1"), "foo", transforms, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) test.That(t, poseResult.FrameName(), test.ShouldEqual, "fooarm1") test.That(t, poseResult.Pose().Point().X, test.ShouldEqual, 1) test.That(t, poseResult.Pose().Point().Y, test.ShouldEqual, 2) test.That(t, poseResult.Pose().Point().Z, test.ShouldEqual, 3) - for key, msg := range msgMap { - receivedMsg := receivedTransforms[key] - receivedPF := receivedMsg.GetPoseInObserverFrame() - msgPF := msg.GetPoseInObserverFrame() - test.That(t, receivedMsg.GetReferenceFrame(), test.ShouldEqual, msg.GetReferenceFrame()) - test.That(t, receivedPF.GetReferenceFrame(), test.ShouldEqual, msgPF.GetReferenceFrame()) - receivedPose := receivedPF.GetPose() - msgPose := msgPF.GetPose() - test.That(t, receivedPose.X, test.ShouldAlmostEqual, msgPose.X) - test.That(t, receivedPose.Y, test.ShouldAlmostEqual, msgPose.Y) - test.That(t, receivedPose.Z, test.ShouldAlmostEqual, msgPose.Z) - test.That(t, receivedPose.OX, test.ShouldAlmostEqual, msgPose.OX) - test.That(t, receivedPose.OY, test.ShouldAlmostEqual, msgPose.OY) - test.That(t, receivedPose.OZ, test.ShouldAlmostEqual, msgPose.OZ) - test.That(t, receivedPose.Theta, test.ShouldAlmostEqual, msgPose.Theta) + for name, tf := range tfMap { + receivedTf := receivedTransforms[name] + test.That(t, tf.Name(), test.ShouldEqual, receivedTf.Name()) + test.That(t, tf.FrameName(), test.ShouldEqual, receivedTf.FrameName()) + test.That(t, spatialmath.PoseAlmostEqual(tf.Pose(), receivedTf.Pose()), test.ShouldBeTrue) } test.That(t, receivedTransforms, test.ShouldNotBeNil) test.That(t, utils.TryClose(context.Background(), client), test.ShouldBeNil) @@ -164,7 +140,7 @@ func TestClient(t *testing.T) { ctx context.Context, componentName resource.Name, grabPose *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { return false, passedErr @@ -174,13 +150,13 @@ func TestClient(t *testing.T) { ctx context.Context, componentName resource.Name, destinationFrame string, - supplementalTransform []*commonpb.Transform, + supplementalTransform []*referenceframe.PoseInFrame, extra map[string]interface{}, ) (*referenceframe.PoseInFrame, error) { return nil, passedErr } - resp, err := client2.Move(context.Background(), resourceName, grabPose, &commonpb.WorldState{}, map[string]interface{}{}) + resp, err := client2.Move(context.Background(), resourceName, grabPose, &referenceframe.WorldState{}, map[string]interface{}{}) test.That(t, err.Error(), test.ShouldContainSubstring, passedErr.Error()) test.That(t, resp, test.ShouldEqual, false) _, err = client2.GetPose(context.Background(), arm.Named("arm1"), "foo", nil, map[string]interface{}{}) diff --git a/services/motion/motion.go b/services/motion/motion.go index 1544a17465d..ce2697abcdc 100644 --- a/services/motion/motion.go +++ b/services/motion/motion.go @@ -6,7 +6,6 @@ import ( "sync" "github.com/edaniels/golog" - commonpb "go.viam.com/api/common/v1" servicepb "go.viam.com/api/service/motion/v1" goutils "go.viam.com/utils" "go.viam.com/utils/rpc" @@ -43,21 +42,21 @@ type Service interface { ctx context.Context, componentName resource.Name, destination *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) MoveSingleComponent( ctx context.Context, componentName resource.Name, destination *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) GetPose( ctx context.Context, componentName resource.Name, destinationFrame string, - supplementalTransforms []*commonpb.Transform, + supplementalTransforms []*referenceframe.PoseInFrame, extra map[string]interface{}, ) (*referenceframe.PoseInFrame, error) } @@ -115,7 +114,7 @@ func (svc *reconfigurableMotionService) Move( ctx context.Context, componentName resource.Name, destination *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { svc.mu.RLock() @@ -127,7 +126,7 @@ func (svc *reconfigurableMotionService) MoveSingleComponent( ctx context.Context, componentName resource.Name, destination *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { svc.mu.RLock() @@ -139,7 +138,7 @@ func (svc *reconfigurableMotionService) GetPose( ctx context.Context, componentName resource.Name, destinationFrame string, - supplementalTransforms []*commonpb.Transform, + supplementalTransforms []*referenceframe.PoseInFrame, extra map[string]interface{}, ) (*referenceframe.PoseInFrame, error) { svc.mu.RLock() diff --git a/services/motion/motion_test.go b/services/motion/motion_test.go index b78a8954dc4..c01d1febbca 100644 --- a/services/motion/motion_test.go +++ b/services/motion/motion_test.go @@ -4,8 +4,6 @@ import ( "context" "testing" - // register. - commonpb "go.viam.com/api/common/v1" "go.viam.com/test" "go.viam.com/rdk/components/gripper" @@ -44,7 +42,7 @@ func (m *mock) Move( ctx context.Context, gripperName resource.Name, grabPose *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { m.grabCount++ @@ -55,7 +53,7 @@ func (m *mock) MoveSingleComponent( ctx context.Context, gripperName resource.Name, grabPose *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { m.grabCount++ @@ -66,7 +64,7 @@ func (m *mock) GetPose( ctx context.Context, componentName resource.Name, destinationFrame string, - supplementalTransforms []*commonpb.Transform, + supplementalTransforms []*referenceframe.PoseInFrame, extra map[string]interface{}, ) (*referenceframe.PoseInFrame, error) { return &referenceframe.PoseInFrame{}, nil @@ -85,7 +83,7 @@ func TestFromRobot(t *testing.T) { test.That(t, svc, test.ShouldNotBeNil) grabPose := referenceframe.NewPoseInFrame("", spatialmath.NewZeroPose()) - result, err := svc.Move(context.Background(), gripper.Named("fake"), grabPose, &commonpb.WorldState{}, map[string]interface{}{}) + result, err := svc.Move(context.Background(), gripper.Named("fake"), grabPose, &referenceframe.WorldState{}, map[string]interface{}{}) test.That(t, err, test.ShouldBeNil) test.That(t, result, test.ShouldEqual, false) test.That(t, svc1.grabCount, test.ShouldEqual, 1) diff --git a/services/motion/server.go b/services/motion/server.go index d2a2cab3448..7dff6e18334 100644 --- a/services/motion/server.go +++ b/services/motion/server.go @@ -41,11 +41,15 @@ func (server *subtypeServer) Move(ctx context.Context, req *pb.MoveRequest) (*pb if err != nil { return nil, err } + worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState()) + if err != nil { + return nil, err + } success, err := svc.Move( ctx, protoutils.ResourceNameFromProto(req.GetComponentName()), referenceframe.ProtobufToPoseInFrame(req.GetDestination()), - req.GetWorldState(), + worldState, req.Extra.AsMap(), ) return &pb.MoveResponse{Success: success}, err @@ -59,11 +63,15 @@ func (server *subtypeServer) MoveSingleComponent( if err != nil { return nil, err } + worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState()) + if err != nil { + return nil, err + } success, err := svc.MoveSingleComponent( ctx, protoutils.ResourceNameFromProto(req.GetComponentName()), referenceframe.ProtobufToPoseInFrame(req.GetDestination()), - req.GetWorldState(), + worldState, req.Extra.AsMap(), ) return &pb.MoveSingleComponentResponse{Success: success}, err @@ -77,13 +85,11 @@ func (server *subtypeServer) GetPose(ctx context.Context, req *pb.GetPoseRequest if req.ComponentName == nil { return nil, errors.New("must provide component name") } - - pose, err := svc.GetPose( - ctx, - protoutils.ResourceNameFromProto(req.ComponentName), - req.DestinationFrame, req.GetSupplementalTransforms(), - req.Extra.AsMap(), - ) + transforms, err := referenceframe.PoseInFramesFromTransformProtobuf(req.GetSupplementalTransforms()) + if err != nil { + return nil, err + } + pose, err := svc.GetPose(ctx, protoutils.ResourceNameFromProto(req.ComponentName), req.DestinationFrame, transforms, req.Extra.AsMap()) if err != nil { return nil, err } diff --git a/services/motion/server_test.go b/services/motion/server_test.go index b44ec8189ba..5800388c011 100644 --- a/services/motion/server_test.go +++ b/services/motion/server_test.go @@ -5,7 +5,6 @@ import ( "errors" "testing" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/service/motion/v1" "go.viam.com/test" @@ -59,7 +58,7 @@ func TestServerMove(t *testing.T) { ctx context.Context, componentName resource.Name, destination *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { return false, passedErr @@ -73,7 +72,7 @@ func TestServerMove(t *testing.T) { ctx context.Context, componentName resource.Name, destination *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { return true, nil @@ -93,7 +92,7 @@ func TestServerMove(t *testing.T) { ctx context.Context, componentName resource.Name, destination *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { return true, nil diff --git a/testutils/inject/arm.go b/testutils/inject/arm.go index 834e3429bb8..c72a3cd4c74 100644 --- a/testutils/inject/arm.go +++ b/testutils/inject/arm.go @@ -3,11 +3,11 @@ package inject import ( "context" - commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/component/arm/v1" "go.viam.com/utils" "go.viam.com/rdk/components/arm" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -16,7 +16,7 @@ type Arm struct { arm.LocalArm 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, ws *commonpb.WorldState, extra map[string]interface{}) error + MoveToPositionFunc func(ctx context.Context, to spatialmath.Pose, ws *referenceframe.WorldState, 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) StopFunc func(ctx context.Context, extra map[string]interface{}) error @@ -36,7 +36,7 @@ func (a *Arm) EndPosition(ctx context.Context, extra map[string]interface{}) (sp func (a *Arm) MoveToPosition( ctx context.Context, to spatialmath.Pose, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { if a.MoveToPositionFunc == nil { diff --git a/testutils/inject/gantry.go b/testutils/inject/gantry.go index 37b7fd65819..0c36c2141d0 100644 --- a/testutils/inject/gantry.go +++ b/testutils/inject/gantry.go @@ -3,7 +3,6 @@ package inject import ( "context" - commonpb "go.viam.com/api/common/v1" "go.viam.com/utils" "go.viam.com/rdk/components/gantry" @@ -15,7 +14,7 @@ type Gantry struct { gantry.LocalGantry DoFunc func(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) PositionFunc func(ctx context.Context, extra map[string]interface{}) ([]float64, error) - MoveToPositionFunc func(ctx context.Context, positions []float64, worldState *commonpb.WorldState, extra map[string]interface{}) error + MoveToPositionFunc func(ctx context.Context, pos []float64, ws *referenceframe.WorldState, extra map[string]interface{}) error LengthsFunc func(ctx context.Context, extra map[string]interface{}) ([]float64, error) StopFunc func(ctx context.Context, extra map[string]interface{}) error IsMovingFunc func(context.Context) (bool, error) @@ -35,7 +34,7 @@ func (g *Gantry) Position(ctx context.Context, extra map[string]interface{}) ([] func (g *Gantry) MoveToPosition( ctx context.Context, positions []float64, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) error { if g.MoveToPositionFunc == nil { diff --git a/testutils/inject/motion_service.go b/testutils/inject/motion_service.go index 161ef80ebcb..be294c92159 100644 --- a/testutils/inject/motion_service.go +++ b/testutils/inject/motion_service.go @@ -3,8 +3,6 @@ package inject import ( "context" - commonpb "go.viam.com/api/common/v1" - "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" @@ -18,14 +16,14 @@ type MotionService struct { ctx context.Context, componentName resource.Name, grabPose *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) GetPoseFunc func( ctx context.Context, componentName resource.Name, destinationFrame string, - supplementalTransforms []*commonpb.Transform, + supplementalTransforms []*referenceframe.PoseInFrame, extra map[string]interface{}, ) (*referenceframe.PoseInFrame, error) } @@ -35,7 +33,7 @@ func (mgs *MotionService) Move( ctx context.Context, componentName resource.Name, grabPose *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { if mgs.MoveFunc == nil { @@ -49,7 +47,7 @@ func (mgs *MotionService) MoveSingleComponent( ctx context.Context, componentName resource.Name, grabPose *referenceframe.PoseInFrame, - worldState *commonpb.WorldState, + worldState *referenceframe.WorldState, extra map[string]interface{}, ) (bool, error) { if mgs.MoveFunc == nil { @@ -63,7 +61,7 @@ func (mgs *MotionService) GetPose( ctx context.Context, componentName resource.Name, destinationFrame string, - supplementalTransforms []*commonpb.Transform, + supplementalTransforms []*referenceframe.PoseInFrame, extra map[string]interface{}, ) (*referenceframe.PoseInFrame, error) { if mgs.GetPoseFunc == nil { diff --git a/testutils/inject/robot.go b/testutils/inject/robot.go index 2fdabc2aa29..a857bc3be18 100644 --- a/testutils/inject/robot.go +++ b/testutils/inject/robot.go @@ -6,7 +6,6 @@ import ( "sync" "github.com/edaniels/golog" - commonpb "go.viam.com/api/common/v1" "go.viam.com/utils" "go.viam.com/utils/pexec" @@ -35,12 +34,12 @@ type Robot struct { CloseFunc func(ctx context.Context) error StopAllFunc func(ctx context.Context, extra map[resource.Name]map[string]interface{}) error RefreshFunc func(ctx context.Context) error - FrameSystemConfigFunc func(ctx context.Context, additionalTransforms []*commonpb.Transform) (framesystemparts.Parts, error) + FrameSystemConfigFunc func(ctx context.Context, additionalTransforms []*referenceframe.PoseInFrame) (framesystemparts.Parts, error) TransformPoseFunc func( ctx context.Context, pose *referenceframe.PoseInFrame, dst string, - additionalTransforms []*commonpb.Transform, + additionalTransforms []*referenceframe.PoseInFrame, ) (*referenceframe.PoseInFrame, error) StatusFunc func(ctx context.Context, resourceNames []resource.Name) ([]robot.Status, error) @@ -203,7 +202,7 @@ func (r *Robot) DiscoverComponents(ctx context.Context, keys []discovery.Query) } // FrameSystemConfig calls the injected FrameSystemConfig or the real version. -func (r *Robot) FrameSystemConfig(ctx context.Context, additionalTransforms []*commonpb.Transform) (framesystemparts.Parts, error) { +func (r *Robot) FrameSystemConfig(ctx context.Context, additionalTransforms []*referenceframe.PoseInFrame) (framesystemparts.Parts, error) { r.Mu.RLock() defer r.Mu.RUnlock() if r.FrameSystemConfigFunc == nil { @@ -218,7 +217,7 @@ func (r *Robot) TransformPose( ctx context.Context, pose *referenceframe.PoseInFrame, dst string, - additionalTransforms []*commonpb.Transform, + additionalTransforms []*referenceframe.PoseInFrame, ) (*referenceframe.PoseInFrame, error) { r.Mu.RLock() defer r.Mu.RUnlock()