RSDK-10936 introduce RobotFrameSystem interface (#5101)

Author: raybjork

go.mod

@@ -81,7 +81,7 @@ require (
 	go.uber.org/atomic v1.11.0
 	go.uber.org/multierr v1.11.0
 	go.uber.org/zap v1.27.0
-	go.viam.com/api v0.1.452
+	go.viam.com/api v0.1.455
 	go.viam.com/test v1.2.4
 	go.viam.com/utils v0.1.150
 	goji.io v2.0.2+incompatible

go.sum

@@ -1513,8 +1513,8 @@ go.uber.org/zap v1.18.1/go.mod h1:xg/QME4nWcxGxrpdeYfq7UvYrLh66cuVKdrbD1XF/NI=
 go.uber.org/zap v1.23.0/go.mod h1:D+nX8jyLsMHMYrln8A0rJjFt/T/9/bGgIhAqxv5URuY=
 go.uber.org/zap v1.27.0 h1:aJMhYGrd5QSmlpLMr2MftRKl7t8J8PTZPA732ud/XR8=
 go.uber.org/zap v1.27.0/go.mod h1:GB2qFLM7cTU87MWRP2mPIjqfIDnGu+VIO4V/SdhGo2E=
-go.viam.com/api v0.1.452 h1:/rexbvi6Bfva/iqNEu2ftS6NrrQICg5CPnj1PKgTAi4=
-go.viam.com/api v0.1.452/go.mod h1:gwJriv6EVWe97uFzzzWjzP3NPfpCrKtRAdWtYglUpqs=
+go.viam.com/api v0.1.455 h1:WY++sFAydRGcg/E1va85BFPi9U7IuEf2Dk0JrPZ1Wzs=
+go.viam.com/api v0.1.455/go.mod h1:gwJriv6EVWe97uFzzzWjzP3NPfpCrKtRAdWtYglUpqs=
 go.viam.com/test v1.2.4 h1:JYgZhsuGAQ8sL9jWkziAXN9VJJiKbjoi9BsO33TW3ug=
 go.viam.com/test v1.2.4/go.mod h1:zI2xzosHdqXAJ/kFqcN+OIF78kQuTV2nIhGZ8EzvaJI=
 go.viam.com/utils v0.1.150 h1:45r/HlGxeAaOQgHakjUhOKFUitOiN0vRlTxkXmzA7iA=

module/modmanager/module.go

@@ -67,7 +67,8 @@ func (m *module) dial() error {
 	if !rutils.TCPRegex.MatchString(addrToDial) {
 		addrToDial = "unix:" + addrToDial
 	}
-	conn, err := grpc.Dial( //nolint:staticcheck
+	//nolint:staticcheck
+	conn, err := grpc.Dial(
 		addrToDial,
 		grpc.WithDefaultCallOptions(grpc.MaxCallRecvMsgSize(rpc.MaxMessageSize)),
 		grpc.WithTransportCredentials(insecure.NewCredentials()),

module/module_test.go

@@ -186,7 +186,8 @@ func TestModuleFunctions(t *testing.T) {
 
 	test.That(t, m.Start(ctx), test.ShouldBeNil)
 
-	conn, err := grpc.Dial( //nolint:staticcheck
+	//nolint:staticcheck
+	conn, err := grpc.Dial(
 		"unix://"+addr,
 		grpc.WithTransportCredentials(insecure.NewCredentials()),
 		grpc.WithStreamInterceptor(grpc_retry.StreamClientInterceptor()),
@@ -477,7 +478,8 @@ func TestAttributeConversion(t *testing.T) {
 		test.That(t, m.AddModelFromRegistry(ctx, shell.API, modelWithReconfigure), test.ShouldBeNil)
 
 		test.That(t, m.Start(ctx), test.ShouldBeNil)
-		conn, err := grpc.Dial( //nolint:staticcheck
+		//nolint:staticcheck
+		conn, err := grpc.Dial(
 			"unix://"+addr,
 			grpc.WithTransportCredentials(insecure.NewCredentials()),
 			grpc.WithStreamInterceptor(grpc_retry.StreamClientInterceptor()),

robot/client/client.go

@@ -973,6 +973,33 @@ func (rc *RobotClient) FrameSystemConfig(ctx context.Context) (*framesystem.Conf
 	return &framesystem.Config{Parts: result}, nil
 }
 
+// GetPose returns the pose of the specified component in the given destination frame.
+func (rc *RobotClient) GetPose(
+	ctx context.Context,
+	componentName, destinationFrame string,
+	supplementalTransforms []*referenceframe.LinkInFrame,
+	extra map[string]interface{},
+) (*referenceframe.PoseInFrame, error) {
+	ext, err := protoutils.StructToStructPb(extra)
+	if err != nil {
+		return nil, err
+	}
+	transforms, err := referenceframe.LinkInFramesToTransformsProtobuf(supplementalTransforms)
+	if err != nil {
+		return nil, err
+	}
+	resp, err := rc.client.GetPose(ctx, &pb.GetPoseRequest{
+		ComponentName:          componentName,
+		DestinationFrame:       destinationFrame,
+		SupplementalTransforms: transforms,
+		Extra:                  ext,
+	})
+	if err != nil {
+		return nil, err
+	}
+	return referenceframe.ProtobufToPoseInFrame(resp.Pose), nil
+}
+
 // TransformPose will transform the pose of the requested poseInFrame to the desired frame in the robot's frame system.
 //
 //	  import (
@@ -986,9 +1013,9 @@ func (rc *RobotClient) TransformPose(
 	ctx context.Context,
 	query *referenceframe.PoseInFrame,
 	destination string,
-	additionalTransforms []*referenceframe.LinkInFrame,
+	supplementalTransforms []*referenceframe.LinkInFrame,
 ) (*referenceframe.PoseInFrame, error) {
-	transforms, err := referenceframe.LinkInFramesToTransformsProtobuf(additionalTransforms)
+	transforms, err := referenceframe.LinkInFramesToTransformsProtobuf(supplementalTransforms)
 	if err != nil {
 		return nil, err
 	}
@@ -1036,6 +1063,27 @@ func (rc *RobotClient) TransformPointCloud(ctx context.Context, srcpc pointcloud
 	return output, nil
 }
 
+// CurrentInputs returns a map of the current inputs for each component of a machine's frame system
+// and a map of statuses indicating which of the machine's components may be actuated through input values.
+func (rc *RobotClient) CurrentInputs(ctx context.Context) (referenceframe.FrameSystemInputs, error) {
+	input := make(referenceframe.FrameSystemInputs)
+	for _, name := range rc.ResourceNames() {
+		res, err := rc.ResourceByName(name)
+		if err != nil {
+			return nil, err
+		}
+		inputEnabled, ok := res.(framesystem.InputEnabled)
+		if ok {
+			pos, err := inputEnabled.CurrentInputs(ctx)
+			if err != nil {
+				return nil, err
+			}
+			input[name.ShortName()] = pos
+		}
+	}
+	return input, nil
+}
+
 // StopAll cancels all current and outstanding operations for the machine and stops all actuators and movement.
 //
 //	err := machine.StopAll(ctx.Background())

robot/client/client_test.go

@@ -2008,6 +2008,67 @@ func TestShutDown(t *testing.T) {
 	test.That(t, shutdownCalled, test.ShouldBeTrue)
 }
 
+func TestCurrentInputs(t *testing.T) {
+	logger := logging.NewTestLogger(t)
+	listener, err := net.Listen("tcp", "localhost:0")
+	test.That(t, err, test.ShouldBeNil)
+	gServer := grpc.NewServer()
+
+	testAPI := resource.APINamespaceRDK.WithComponentType(arm.SubtypeName)
+	testName := resource.NewName(testAPI, "arm1")
+	testName2 := resource.NewName(testAPI, "arm2")
+
+	expectedInputs := referenceframe.FrameSystemInputs{
+		testName.ShortName():  []referenceframe.Input{{0}, {math.Pi}, {-math.Pi}, {0}, {math.Pi}, {-math.Pi}},
+		testName2.ShortName(): []referenceframe.Input{{math.Pi}, {-math.Pi}, {0}, {math.Pi}, {-math.Pi}, {0}},
+	}
+	injectArm := &inject.Arm{
+		JointPositionsFunc: func(ctx context.Context, extra map[string]any) ([]referenceframe.Input, error) {
+			return expectedInputs[testName.ShortName()], nil
+		},
+		KinematicsFunc: func(ctx context.Context) (referenceframe.Model, error) {
+			return referenceframe.ParseModelJSONFile(rutils.ResolveFile("components/arm/example_kinematics/ur5e.json"), "")
+		},
+	}
+	injectArm2 := &inject.Arm{
+		JointPositionsFunc: func(ctx context.Context, extra map[string]any) ([]referenceframe.Input, error) {
+			return expectedInputs[testName2.ShortName()], nil
+		},
+		KinematicsFunc: func(ctx context.Context) (referenceframe.Model, error) {
+			return referenceframe.ParseModelJSONFile(rutils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "")
+		},
+	}
+	resourceNames := []resource.Name{testName, testName2}
+	resources := map[resource.Name]arm.Arm{testName: injectArm, testName2: injectArm2}
+	injectRobot := &inject.Robot{
+		ResourceNamesFunc:  func() []resource.Name { return resourceNames },
+		ResourceByNameFunc: func(n resource.Name) (resource.Resource, error) { return resources[n], nil },
+		MachineStatusFunc: func(ctx context.Context) (robot.MachineStatus, error) {
+			return robot.MachineStatus{State: robot.StateRunning}, nil
+		},
+		ResourceRPCAPIsFunc: func() []resource.RPCAPI { return nil },
+	}
+
+	armSvc, err := resource.NewAPIResourceCollection(arm.API, resources)
+	test.That(t, err, test.ShouldBeNil)
+	gServer.RegisterService(&armpb.ArmService_ServiceDesc, arm.NewRPCServiceServer(armSvc))
+	pb.RegisterRobotServiceServer(gServer, server.New(injectRobot))
+
+	go gServer.Serve(listener)
+	defer gServer.Stop()
+
+	client, err := New(context.Background(), listener.Addr().String(), logger)
+	test.That(t, err, test.ShouldBeNil)
+	defer func() {
+		test.That(t, client.Close(context.Background()), test.ShouldBeNil)
+	}()
+
+	inputs, err := client.CurrentInputs(context.Background())
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, len(inputs), test.ShouldEqual, 2)
+	test.That(t, inputs, test.ShouldResemble, expectedInputs)
+}
+
 func TestUnregisteredResourceByName(t *testing.T) {
 	logger := logging.NewTestLogger(t)
 	listener, err := net.Listen("tcp", "localhost:0")