DATA-841 Add rdk representation of worldstate along with conversion methods

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
@@ -214,7 +213,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()
@@ -345,7 +344,7 @@ func WrapWithReconfigurable(r interface{}) (resource.Reconfigurable, error) {
 }
 
 // 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
@@ -360,10 +359,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
 	}

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

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)
 

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)

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)

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(),
 	)
 }

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

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)

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) {

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)
 }

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)

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)

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)

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
 	}