wip

Author: raybjork

components/arm/client.go

@@ -23,6 +23,7 @@ type client struct {
 	resource.TriviallyCloseable
 	name   string
 	client pb.ArmServiceClient
+	model  referenceframe.Model
 	logger logging.Logger
 }
 
@@ -35,12 +36,24 @@ func NewClientFromConn(
 	logger logging.Logger,
 ) (Arm, error) {
 	pbClient := pb.NewArmServiceClient(conn)
-	return &client{
+	c := &client{
 		Named:  name.PrependRemote(remoteName).AsNamed(),
 		name:   name.ShortName(),
 		client: pbClient,
 		logger: logger,
-	}, nil
+	}
+	clientFrame, err := c.Kinematics(ctx)
+	if err != nil {
+		logger.CWarnw(
+			ctx,
+			"error getting model for arm; making the assumption that joints are revolute and that their positions are specified in degrees",
+			"err",
+			err,
+		)
+	} else {
+		c.model = clientFrame
+	}
+	return c, nil
 }
 
 func (c *client) EndPosition(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) {
@@ -79,12 +92,7 @@ func (c *client) MoveToJointPositions(ctx context.Context, positions []reference
 	if err != nil {
 		return err
 	}
-	m, err := c.Kinematics(ctx)
-	if err != nil {
-		return err
-	}
-
-	jp, err := referenceframe.JointPositionsFromInputs(m, positions)
+	jp, err := referenceframe.JointPositionsFromInputs(c.model, positions)
 	if err != nil {
 		return err
 	}
@@ -110,12 +118,8 @@ func (c *client) MoveThroughJointPositions(
 		c.logger.Warnf("%s MoveThroughJointPositions: position argument is nil", c.name)
 	}
 	allJPs := make([]*pb.JointPositions, 0, len(positions))
-	m, err := c.Kinematics(ctx)
-	if err != nil {
-		return err
-	}
 	for _, position := range positions {
-		jp, err := referenceframe.JointPositionsFromInputs(m, position)
+		jp, err := referenceframe.JointPositionsFromInputs(c.model, position)
 		if err != nil {
 			return err
 		}
@@ -145,11 +149,7 @@ func (c *client) JointPositions(ctx context.Context, extra map[string]interface{
 	if err != nil {
 		return nil, err
 	}
-	m, err := c.Kinematics(ctx)
-	if err != nil {
-		return nil, err
-	}
-	return referenceframe.InputsFromJointPositions(m, resp.Positions)
+	return referenceframe.InputsFromJointPositions(c.model, resp.Positions)
 }
 
 func (c *client) Stop(ctx context.Context, extra map[string]interface{}) error {

components/arm/client_test.go

@@ -2,7 +2,6 @@ package arm_test
 
 import (
 	"context"
-	"errors"
 	"net"
 	"testing"
 
@@ -83,7 +82,7 @@ func TestClient(t *testing.T) {
 		return errStopUnimplemented
 	}
 	injectArm.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
-		return nil, errors.New("KinematicsFunc unimplemented")
+		return nil, errKinematicsUnimplemented
 	}
 	injectArm.GeometriesFunc = func(ctx context.Context) ([]spatialmath.Geometry, error) {
 		return expectedGeometries, nil
@@ -110,7 +109,7 @@ func TestClient(t *testing.T) {
 		return nil
 	}
 	injectArm2.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
-		return nil, errors.New("KinematicsFunc unimplemented")
+		return nil, errKinematicsUnimplemented
 	}
 
 	armSvc, err := resource.NewAPIResourceCollection(arm.API, map[resource.Name]arm.Arm{

components/arm/server.go

@@ -66,7 +66,6 @@ func (s *serviceServer) GetJointPositions(ctx context.Context, req *pb.GetJointP
 	if err != nil {
 		return nil, err
 	}
-
 	jp, err := referenceframe.JointPositionsFromInputs(m, pos)
 	if err != nil {
 		return nil, err

components/arm/server_test.go

@@ -26,6 +26,7 @@ var (
 	errMoveToJointPositionFailed = errors.New("can't move to joint positions")
 	errStopUnimplemented         = errors.New("Stop unimplemented")
 	errArmUnimplemented          = errors.New("not found")
+	errKinematicsUnimplemented   = errors.New("kinematics unimplemented")
 )
 
 func newServer() (pb.ArmServiceServer, *inject.Arm, *inject.Arm, error) {
@@ -114,7 +115,7 @@ func TestServer(t *testing.T) {
 		return errMoveToJointPositionFailed
 	}
 	injectArm2.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
-		return nil, errors.New("KinematicsFunc unimplemented")
+		return nil, errKinematicsUnimplemented
 	}
 	injectArm2.StopFunc = func(ctx context.Context, extra map[string]interface{}) error {
 		return errStopUnimplemented
@@ -249,8 +250,8 @@ func TestServer(t *testing.T) {
 		test.That(t, kinematics.Format, test.ShouldResemble, commonpb.KinematicsFileFormat_KINEMATICS_FILE_FORMAT_URDF)
 
 		kinematics, err = armServer.GetKinematics(context.Background(), &commonpb.GetKinematicsRequest{Name: failArmName})
-		test.That(t, err, test.ShouldBeNil)
-		test.That(t, kinematics.Format, test.ShouldResemble, commonpb.KinematicsFileFormat_KINEMATICS_FILE_FORMAT_UNSPECIFIED)
+		test.That(t, err, test.ShouldNotBeNil)
+		test.That(t, err, test.ShouldResemble, errKinematicsUnimplemented)
 	})
 
 	t.Run("stop", func(t *testing.T) {