[RSDK-12679] Implement GetKinematics and GetGeometries for Gantry in Go SDK

components/gantry/client.go

@@ -3,8 +3,9 @@ package gantry
 
 import (
 	"context"
-	"errors"
+	"sync"
 
+	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"
@@ -13,16 +14,21 @@ import (
 	rprotoutils "go.viam.com/rdk/protoutils"
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/resource"
+	"go.viam.com/rdk/spatialmath"
 )
 
 // client implements GantryServiceClient.
 type client struct {
 	resource.Named
+	resource.Shaped
 	resource.TriviallyReconfigurable
 	resource.TriviallyCloseable
 	name   string
 	client pb.GantryServiceClient
 	logger logging.Logger
+
+	mu    sync.Mutex
+	model referenceframe.Model
 }
 
 // NewClientFromConn constructs a new Client from connection passed in.
@@ -118,8 +124,37 @@ func (c *client) Stop(ctx context.Context, extra map[string]interface{}) error {
 	return err
 }
 
+func (c *client) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) {
+	ext, err := protoutils.StructToStructPb(extra)
+	if err != nil {
+		return nil, err
+	}
+	resp, err := c.client.GetGeometries(ctx, &commonpb.GetGeometriesRequest{
+		Name:  c.name,
+		Extra: ext,
+	})
+	if err != nil {
+		return nil, err
+	}
+	return referenceframe.NewGeometriesFromProto(resp.GetGeometries())
+}
+
 func (c *client) Kinematics(ctx context.Context) (referenceframe.Model, error) {
-	return nil, errors.New("unimplemented")
+	c.mu.Lock()
+	defer c.mu.Unlock()
+
+	if c.model == nil {
+		resp, err := c.client.GetKinematics(ctx, &commonpb.GetKinematicsRequest{Name: c.name})
+		if err != nil {
+			return nil, err
+		}
+		model, err := referenceframe.KinematicModelFromProtobuf(c.name, resp)
+		if err != nil {
+			return nil, err
+		}
+		c.model = model
+	}
+	return c.model, nil
 }
 
 func (c *client) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) {

components/gantry/client_test.go

@@ -11,6 +11,7 @@ import (
 	"go.viam.com/rdk/components/gantry"
 	viamgrpc "go.viam.com/rdk/grpc"
 	"go.viam.com/rdk/logging"
+	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/testutils"
 	"go.viam.com/rdk/testutils/inject"
@@ -52,6 +53,13 @@ func TestClient(t *testing.T) {
 		extra1 = extra
 		return true, nil
 	}
+	injectGantry.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
+		model, err := referenceframe.KinematicModelFromFile("./test_gantry_model.json", "test_gantry_model")
+		if err != nil {
+			return nil, err
+		}
+		return model, nil
+	}
 
 	pos2 := []float64{4.0, 5.0, 6.0}
 	speed2 := []float64{100.0, 80.0, 120.0}
@@ -80,6 +88,9 @@ func TestClient(t *testing.T) {
 		extra2 = extra
 		return false, errHomingFailed
 	}
+	injectGantry2.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
+		return nil, errKinematicsFailed
+	}
 
 	gantrySvc, err := resource.NewAPIResourceCollection(
 		gantry.API,
@@ -139,6 +150,10 @@ func TestClient(t *testing.T) {
 		test.That(t, homed, test.ShouldBeTrue)
 		test.That(t, extra1, test.ShouldResemble, map[string]interface{}{"foo": 345., "bar": "456"})
 
+		model, err := gantry1Client.Kinematics(context.Background())
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, model, test.ShouldNotBeNil)
+
 		err = gantry1Client.Stop(context.Background(), map[string]interface{}{"foo": 456, "bar": "567"})
 		test.That(t, err, test.ShouldNotBeNil)
 		test.That(t, err.Error(), test.ShouldContainSubstring, errStopFailed.Error())
@@ -164,6 +179,10 @@ func TestClient(t *testing.T) {
 		test.That(t, homed, test.ShouldBeFalse)
 		test.That(t, extra2, test.ShouldResemble, map[string]interface{}{"foo": 345., "bar": "456"})
 
+		model, err := client2.Kinematics(context.Background())
+		test.That(t, err.Error(), test.ShouldContainSubstring, errKinematicsFailed.Error())
+		test.That(t, model, test.ShouldBeNil)
+
 		err = client2.Stop(context.Background(), map[string]interface{}{"foo": "234", "bar": 345})
 		test.That(t, err, test.ShouldBeNil)
 		test.That(t, extra2, test.ShouldResemble, map[string]interface{}{"foo": "234", "bar": 345.})

components/gantry/fake/gantry.go

@@ -3,13 +3,15 @@ package fake
 
 import (
 	"context"
+	"fmt"
 
 	"github.com/golang/geo/r3"
 
 	"go.viam.com/rdk/components/gantry"
 	"go.viam.com/rdk/logging"
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/resource"
+	"go.viam.com/rdk/spatialmath"
 	"go.viam.com/rdk/testutils"
 )
 
@@ -31,15 +33,33 @@ func init() {
 
 // NewGantry returns a new fake gantry.
 func NewGantry(name resource.Name, logger logging.Logger) gantry.Gantry {
+	m := referenceframe.NewSimpleModel("test_gantry")
+	pose := spatialmath.NewZeroPose()
+	carriageGeom, err := spatialmath.NewBox(pose, r3.Vector{150, 150, 10}, "carriage")
+	if err != nil {
+		logger.CErrorf(context.Background(), "could not create carriage geometry: %v", err)
+	}
+	f, err := referenceframe.NewStaticFrameWithGeometry("carriage", pose, carriageGeom)
+	if err != nil {
+		logger.CErrorf(context.Background(), "could not create static frame: %v", err)
+	}
+	m.SetOrdTransforms(append(m.OrdTransforms(), f))
+
+	f, err = referenceframe.NewTranslationalFrame(
+		"gantry_joint", r3.Vector{1, 0, 0}, referenceframe.Limit{Min: 0, Max: 350})
+	if err != nil {
+		logger.CErrorf(context.Background(), "could not create translational frame: %v", err)
+	}
+	m.SetOrdTransforms(append(m.OrdTransforms(), f))
+
 	return &Gantry{
 		testutils.NewUnimplementedResource(name),
 		resource.TriviallyReconfigurable{},
 		resource.TriviallyCloseable{},
-		[]float64{1.2},
 		[]float64{120},
-		[]float64{5},
-		2,
-		r3.Vector{X: 1, Y: 0, Z: 0},
+		[]float64{50},
+		[]float64{350},
+		m,
 		logger,
 	}
 }
@@ -51,9 +71,8 @@ type Gantry struct {
 	resource.TriviallyCloseable
 	positionsMm    []float64
 	speedsMmPerSec []float64
-	lengths        []float64
-	lengthMeters   float64
-	frame          r3.Vector
+	lengthsMm      []float64
+	model          referenceframe.Model
 	logger         logging.Logger
 }
 
@@ -64,7 +83,7 @@ func (g *Gantry) Position(ctx context.Context, extra map[string]interface{}) ([]
 
 // Lengths returns the position in meters.
 func (g *Gantry) Lengths(ctx context.Context, extra map[string]interface{}) ([]float64, error) {
-	return g.lengths, nil
+	return g.lengthsMm, nil
 }
 
 // Home runs the homing sequence of the gantry and returns true once completed.
@@ -75,6 +94,12 @@ func (g *Gantry) Home(ctx context.Context, extra map[string]interface{}) (bool,
 
 // MoveToPosition is in meters.
 func (g *Gantry) MoveToPosition(ctx context.Context, positionsMm, speedsMmPerSec []float64, extra map[string]interface{}) error {
+	for i, position := range positionsMm {
+		if position < 0 || position > g.lengthsMm[i] {
+			return fmt.Errorf("position %v out of range [0, %v]", position, g.lengthsMm[i])
+		}
+	}
+
 	g.positionsMm = positionsMm
 	g.speedsMmPerSec = speedsMmPerSec
 	return nil
@@ -90,15 +115,22 @@ func (g *Gantry) IsMoving(ctx context.Context) (bool, error) {
 	return false, nil
 }
 
-// Kinematics returns the kinematic model associated with the gantry.
-func (g *Gantry) Kinematics(ctx context.Context) (referenceframe.Model, error) {
-	m := referenceframe.NewSimpleModel("")
-	f, err := referenceframe.NewTranslationalFrame(g.Name().ShortName(), g.frame, referenceframe.Limit{0, g.lengthMeters})
+// Geometries returns the geometries of the gantry.
+func (g *Gantry) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) {
+	inputs, err := g.CurrentInputs(ctx)
 	if err != nil {
 		return nil, err
 	}
-	m.SetOrdTransforms(append(m.OrdTransforms(), f))
-	return m, nil
+	gif, err := g.model.Geometries(inputs)
+	if err != nil {
+		return nil, err
+	}
+	return gif.Geometries(), nil
+}
+
+// Kinematics returns the kinematic model associated with the gantry.
+func (g *Gantry) Kinematics(ctx context.Context) (referenceframe.Model, error) {
+	return g.model, nil
 }
 
 // CurrentInputs returns positions in the Gantry frame model..

components/gantry/gantry.go

@@ -98,6 +98,7 @@ func Named(name string) resource.Name {
 // [Home method docs]: https://docs.viam.com/dev/reference/apis/components/gantry/#home
 type Gantry interface {
 	resource.Resource
+	resource.Shaped
 	resource.Actuator
 	framesystem.InputEnabled
 

components/gantry/multiaxis/multiaxis.go

@@ -14,6 +14,7 @@ import (
 	"go.viam.com/rdk/operation"
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/resource"
+	"go.viam.com/rdk/spatialmath"
 	rdkutils "go.viam.com/rdk/utils"
 )
 
@@ -91,6 +92,16 @@ func newMultiAxis(
 		return nil, err
 	}
 
+	model := referenceframe.NewSimpleModel(mAx.Name().Name)
+	for _, subAx := range mAx.subAxes {
+		k, err := subAx.Kinematics(ctx)
+		if err != nil {
+			return nil, err
+		}
+		model.SetOrdTransforms(append(model.OrdTransforms(), k))
+	}
+	mAx.model = model
+
 	return mAx, nil
 }
 
@@ -229,18 +240,19 @@ func (g *multiAxis) IsMoving(ctx context.Context) (bool, error) {
 	return g.opMgr.OpRunning(), nil
 }
 
-func (g *multiAxis) Kinematics(ctx context.Context) (referenceframe.Model, error) {
-	if g.model == nil {
-		model := referenceframe.NewSimpleModel("")
-		for _, subAx := range g.subAxes {
-			k, err := subAx.Kinematics(ctx)
-			if err != nil {
-				return nil, err
-			}
-			model.SetOrdTransforms(append(model.OrdTransforms(), k))
-		}
-		g.model = model
+func (g *multiAxis) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) {
+	inputs, err := g.CurrentInputs(ctx)
+	if err != nil {
+		return nil, err
+	}
+	gif, err := g.model.Geometries(inputs)
+	if err != nil {
+		return nil, err
 	}
+	return gif.Geometries(), nil
+}
+
+func (g *multiAxis) Kinematics(ctx context.Context) (referenceframe.Model, error) {
 	return g.model, nil
 }
 

components/gantry/multiaxis/multiaxis_test.go

@@ -48,14 +48,23 @@ func createFakeDeps() resource.Dependencies {
 	fakeGantry1.LengthsFunc = func(ctx context.Context, extra map[string]interface{}) ([]float64, error) {
 		return []float64{1}, nil
 	}
+	fakeGantry1.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
+		return referenceframe.NewSimpleModel(""), nil
+	}
 	fakeGantry2 := inject.NewGantry("2")
 	fakeGantry2.LengthsFunc = func(ctx context.Context, extra map[string]interface{}) ([]float64, error) {
 		return []float64{1}, nil
 	}
+	fakeGantry2.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
+		return referenceframe.NewSimpleModel(""), nil
+	}
 	fakeGantry3 := inject.NewGantry("3")
 	fakeGantry3.LengthsFunc = func(ctx context.Context, extra map[string]interface{}) ([]float64, error) {
 		return []float64{1}, nil
 	}
+	fakeGantry3.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
+		return referenceframe.NewSimpleModel(""), nil
+	}
 	fakeMotor := &fm.Motor{
 		Named: motor.Named("fm1").AsNamed(),
 	}
@@ -305,6 +314,7 @@ func TestKinematics(t *testing.T) {
 		Named:     gantry.Named("foo").AsNamed(),
 		subAxes:   twoAxes,
 		lengthsMm: []float64{1, 1},
+		model:     referenceframe.NewSimpleModel(""),
 		opMgr:     operation.NewSingleOperationManager(),
 	}
 	model, err := fakemultiaxis.Kinematics(context.Background())
@@ -315,6 +325,7 @@ func TestKinematics(t *testing.T) {
 		Named:     gantry.Named("foo").AsNamed(),
 		subAxes:   threeAxes,
 		lengthsMm: []float64{1, 1, 1},
+		model:     referenceframe.NewSimpleModel(""),
 		opMgr:     operation.NewSingleOperationManager(),
 	}
 	model, err = fakemultiaxis.Kinematics(context.Background())
@@ -342,6 +353,9 @@ func createComplexDeps() resource.Dependencies {
 	mAx1.StopFunc = func(ctx context.Context, extra map[string]interface{}) error {
 		return nil
 	}
+	mAx1.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
+		return referenceframe.NewSimpleModel("mAx1"), nil
+	}
 
 	position2 := []float64{9, 8, 7}
 	mAx2 := inject.NewGantry("2")
@@ -362,6 +376,9 @@ func createComplexDeps() resource.Dependencies {
 	mAx2.StopFunc = func(ctx context.Context, extra map[string]interface{}) error {
 		return nil
 	}
+	mAx2.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
+		return referenceframe.NewSimpleModel("mAx2"), nil
+	}
 
 	fakeMotor := &fm.Motor{
 		Named: motor.Named("foo").AsNamed(),