Translational arm joint support (#996)

* Initial work on protobuf frame * Support for translational joints everywhere except client * Frames now know what sort of inputs they are expecting, and are able to convert protobuf floats to those inputs * use model instead of repeated ModelFrame calls * lint * make input err a variable
viamrobotics · Jul 8, 2022 · 0398621 · 0398621
1 parent 23ccbc2
commit 0398621
Show file tree

Hide file tree

Showing 24 changed files with 182 additions and 60 deletions.
diff --git a/component/arm/arm.go b/component/arm/arm.go
@@ -405,8 +405,13 @@ func Move(
 	}
 	logger.Debugf("frame system inputs: %v", inputs)
 
+	model := a.ModelFrame()
+	if model == nil {
+		return errors.New("arm did not provide a valid model")
+	}
+
 	// conduct planning query
-	mp, err := motionplan.NewCBiRRTMotionPlanner(a.ModelFrame(), numCPUs, logger)
+	mp, err := motionplan.NewCBiRRTMotionPlanner(model, numCPUs, logger)
 	if err != nil {
 		return err
 	}
@@ -415,7 +420,8 @@ func Move(
 	if err != nil {
 		return err
 	}
-	seedPos, err := a.ModelFrame().Transform(referenceframe.JointPosToInputs(seed))
+
+	seedPos, err := model.Transform(model.InputFromProtobuf(seed))
 	if err != nil {
 		return err
 	}
@@ -425,23 +431,23 @@ func Move(
 	goals := make([]spatialmath.Pose, 0, numSteps)
 	opts := make([]*motionplan.PlannerOptions, 0, numSteps)
 
-	collisionConst := motionplan.NewCollisionConstraintFromWorldState(a.ModelFrame(), fs, worldState, inputs)
+	collisionConst := motionplan.NewCollisionConstraintFromWorldState(model, fs, worldState, inputs)
 
 	from := seedPos
 	for i := 1; i < numSteps; i++ {
 		by := float64(i) / float64(numSteps)
 		to := spatialmath.Interpolate(seedPos, goalPos, by)
 		goals = append(goals, to)
-		opt := DefaultArmPlannerOptions(from, to, a.ModelFrame(), collisionConst)
+		opt := DefaultArmPlannerOptions(from, to, model, collisionConst)
 		opts = append(opts, opt)
 
 		from = to
 	}
 	goals = append(goals, goalPos)
-	opt := DefaultArmPlannerOptions(from, goalPos, a.ModelFrame(), collisionConst)
+	opt := DefaultArmPlannerOptions(from, goalPos, model, collisionConst)
 	opts = append(opts, opt)
 
-	solution, err := motionplan.RunPlannerWithWaypoints(ctx, mp, goals, referenceframe.JointPosToInputs(seed), opts, 0)
+	solution, err := motionplan.RunPlannerWithWaypoints(ctx, mp, goals, model.InputFromProtobuf(seed), opts, 0)
 	if err != nil {
 		return err
 	}

diff --git a/component/arm/client.go b/component/arm/client.go
@@ -3,6 +3,7 @@ package arm
 
 import (
 	"context"
+	"errors"
 
 	"github.com/edaniels/golog"
 	"go.viam.com/utils/rpc"
@@ -13,6 +14,8 @@ import (
 	"go.viam.com/rdk/referenceframe"
 )
 
+var errArmClientInputsNotSupport = errors.New("arm client does not support inputs directly")
+
 // serviceClient is a client satisfies the arm.proto contract.
 type serviceClient struct {
 	conn   rpc.ClientConn
@@ -97,15 +100,11 @@ func (c *client) ModelFrame() referenceframe.Model {
 }
 
 func (c *client) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) {
-	res, err := c.GetJointPositions(ctx)
-	if err != nil {
-		return nil, err
-	}
-	return referenceframe.JointPosToInputs(res), nil
+	return nil, errArmClientInputsNotSupport
 }
 
 func (c *client) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
-	return c.MoveToJointPositions(ctx, referenceframe.InputsToJointPos(goal))
+	return errArmClientInputsNotSupport
 }
 
 func (c *client) Do(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) {

diff --git a/component/arm/eva/eva.go b/component/arm/eva/eva.go
@@ -347,11 +347,11 @@ func (e *eva) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error)
 	if err != nil {
 		return nil, err
 	}
-	return referenceframe.JointPosToInputs(res), nil
+	return e.model.InputFromProtobuf(res), nil
 }
 
 func (e *eva) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
-	return e.MoveToJointPositions(ctx, referenceframe.InputsToJointPos(goal))
+	return e.MoveToJointPositions(ctx, e.model.ProtobufFromInput(goal))
 }
 
 // EvaModel() returns the kinematics model of the Eva, also has all Frame information.

diff --git a/component/arm/fake/arm.go b/component/arm/fake/arm.go
@@ -100,12 +100,12 @@ func (a *Arm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error)
 	if err != nil {
 		return nil, err
 	}
-	return referenceframe.JointPosToInputs(res), nil
+	return a.model.InputFromProtobuf(res), nil
 }
 
 // GoToInputs TODO.
 func (a *Arm) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
-	return a.MoveToJointPositions(ctx, referenceframe.InputsToJointPos(goal))
+	return a.MoveToJointPositions(ctx, a.model.ProtobufFromInput(goal))
 }
 
 // Close does nothing.

diff --git a/component/arm/fake/arm_ik.go b/component/arm/fake/arm_ik.go
@@ -89,7 +89,7 @@ func (a *ArmIK) MoveToPosition(ctx context.Context, pos *commonpb.Pose, worldSta
 	if err != nil {
 		return err
 	}
-	solution, err := a.mp.Plan(ctx, pos, referenceframe.JointPosToInputs(joints), nil)
+	solution, err := a.mp.Plan(ctx, pos, a.model.InputFromProtobuf(joints), nil)
 	if err != nil {
 		return err
 	}
@@ -123,12 +123,12 @@ func (a *ArmIK) CurrentInputs(ctx context.Context) ([]referenceframe.Input, erro
 	if err != nil {
 		return nil, err
 	}
-	return referenceframe.JointPosToInputs(res), nil
+	return a.model.InputFromProtobuf(res), nil
 }
 
 // GoToInputs TODO.
 func (a *ArmIK) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
-	return a.MoveToJointPositions(ctx, referenceframe.InputsToJointPos(goal))
+	return a.MoveToJointPositions(ctx, a.model.ProtobufFromInput(goal))
 }
 
 // Close does nothing.

diff --git a/component/arm/universalrobots/ur.go b/component/arm/universalrobots/ur.go
@@ -344,12 +344,12 @@ func (ua *URArm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, err
 	if err != nil {
 		return nil, err
 	}
-	return referenceframe.JointPosToInputs(res), nil
+	return ua.model.InputFromProtobuf(res), nil
 }
 
 // GoToInputs TODO.
 func (ua *URArm) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
-	return ua.MoveToJointPositions(ctx, referenceframe.InputsToJointPos(goal))
+	return ua.MoveToJointPositions(ctx, ua.model.ProtobufFromInput(goal))
 }
 
 // AddToLog TODO.

diff --git a/component/arm/varm/v1.go b/component/arm/varm/v1.go
@@ -329,11 +329,11 @@ func (a *armV1) CurrentInputs(ctx context.Context) ([]referenceframe.Input, erro
 	if err != nil {
 		return nil, err
 	}
-	return referenceframe.JointPosToInputs(res), nil
+	return a.model.InputFromProtobuf(res), nil
 }
 
 func (a *armV1) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
-	return a.MoveToJointPositions(ctx, referenceframe.InputsToJointPos(goal))
+	return a.MoveToJointPositions(ctx, a.model.ProtobufFromInput(goal))
 }
 
 func computeInnerJointAngle(j0, j1 float64) float64 {

diff --git a/component/arm/vx300s/vx300s.go b/component/arm/vx300s/vx300s.go
@@ -407,11 +407,11 @@ func (a *myArm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, erro
 	if err != nil {
 		return nil, err
 	}
-	return referenceframe.JointPosToInputs(res), nil
+	return a.model.InputFromProtobuf(res), nil
 }
 
 func (a *myArm) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
-	return a.MoveToJointPositions(ctx, referenceframe.InputsToJointPos(goal))
+	return a.MoveToJointPositions(ctx, a.model.ProtobufFromInput(goal))
 }
 
 // TODO: Map out *all* servo defaults so that they are always set correctly.

diff --git a/component/arm/wrapper/wrapper.go b/component/arm/wrapper/wrapper.go
@@ -126,10 +126,10 @@ func (wrapper *Arm) CurrentInputs(ctx context.Context) ([]referenceframe.Input,
 	if err != nil {
 		return nil, err
 	}
-	return referenceframe.JointPosToInputs(res), nil
+	return wrapper.model.InputFromProtobuf(res), nil
 }
 
 // GoToInputs moves the arm to the specified goal inputs.
 func (wrapper *Arm) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
-	return wrapper.MoveToJointPositions(ctx, referenceframe.InputsToJointPos(goal))
+	return wrapper.MoveToJointPositions(ctx, wrapper.model.ProtobufFromInput(goal))
 }
diff --git a/component/arm/wx250s/wx250s.go b/component/arm/wx250s/wx250s.go
@@ -371,12 +371,12 @@ func (a *Arm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error)
 	if err != nil {
 		return nil, err
 	}
-	return referenceframe.JointPosToInputs(res), nil
+	return a.model.InputFromProtobuf(res), nil
 }
 
 // GoToInputs TODO.
 func (a *Arm) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
-	return a.MoveToJointPositions(ctx, referenceframe.InputsToJointPos(goal))
+	return a.MoveToJointPositions(ctx, a.model.ProtobufFromInput(goal))
 }
 
 // WaitForMovement takes some servos, and will block until the servos are done moving.

diff --git a/component/arm/xarm/xarm.go b/component/arm/xarm/xarm.go
@@ -152,11 +152,11 @@ func (x *xArm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error
 	if err != nil {
 		return nil, err
 	}
-	return referenceframe.JointPosToInputs(res), nil
+	return x.model.InputFromProtobuf(res), nil
 }
 
 func (x *xArm) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
-	return x.MoveToJointPositions(ctx, referenceframe.InputsToJointPos(goal))
+	return x.MoveToJointPositions(ctx, x.model.ProtobufFromInput(goal))
 }
 
 // ModelFrame returns the dynamic frame of the model.

diff --git a/component/arm/xarm/xarm_comm.go b/component/arm/xarm/xarm_comm.go
@@ -348,12 +348,12 @@ func (x *xArm) MoveToJointPositions(ctx context.Context, newPositions *pb.JointP
 			return err
 		}
 	}
-	to := referenceframe.JointPosToInputs(newPositions)
+	to := x.model.InputFromProtobuf(newPositions)
 	curPos, err := x.GetJointPositions(ctx)
 	if err != nil {
 		return err
 	}
-	from := referenceframe.JointPosToInputs(curPos)
+	from := x.model.InputFromProtobuf(curPos)
 
 	diff := getMaxDiff(from, to)
 	nSteps := int((diff / float64(x.speed)) * x.moveHZ)

diff --git a/component/arm/yahboom/dofbot.go b/component/arm/yahboom/dofbot.go
@@ -364,12 +364,12 @@ func (a *Dofbot) CurrentInputs(ctx context.Context) ([]referenceframe.Input, err
 	if err != nil {
 		return nil, err
 	}
-	return referenceframe.JointPosToInputs(res), nil
+	return a.model.InputFromProtobuf(res), nil
 }
 
 // GoToInputs moves the arm to the specified goal inputs.
 func (a *Dofbot) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
-	return a.MoveToJointPositions(ctx, referenceframe.InputsToJointPos(goal))
+	return a.MoveToJointPositions(ctx, a.model.ProtobufFromInput(goal))
 }
 
 // Close closes the arm.

diff --git a/component/arm/yahboom/dofbot_test.go b/component/arm/yahboom/dofbot_test.go
@@ -10,7 +10,6 @@ import (
 	"go.viam.com/rdk/motionplan"
 	commonpb "go.viam.com/rdk/proto/api/common/v1"
 	componentpb "go.viam.com/rdk/proto/api/component/arm/v1"
-	"go.viam.com/rdk/referenceframe"
 )
 
 func TestJointConfig(t *testing.T) {
@@ -34,6 +33,6 @@ func TestDofBotIK(t *testing.T) {
 	goal := commonpb.Pose{X: 206.59, Y: -1.57, Z: 253.05, Theta: -180, OX: -.53, OY: 0, OZ: .85}
 	opt := motionplan.NewDefaultPlannerOptions()
 	opt.SetMetric(motionplan.NewPositionOnlyMetric())
-	_, err = mp.Plan(ctx, &goal, referenceframe.JointPosToInputs(&componentpb.JointPositions{Degrees: make([]float64, 5)}), opt)
+	_, err = mp.Plan(ctx, &goal, model.InputFromProtobuf(&componentpb.JointPositions{Degrees: make([]float64, 5)}), opt)
 	test.That(t, err, test.ShouldBeNil)
 }
diff --git a/motionplan/constraint_test.go b/motionplan/constraint_test.go
@@ -157,8 +157,8 @@ func TestLineFollow(t *testing.T) {
 	opt.AddConstraint("whiteboard", validFunc)
 	ok, lastGood := opt.CheckConstraintPath(
 		&ConstraintInput{
-			StartInput: frame.JointPosToInputs(mp1),
-			EndInput:   frame.JointPosToInputs(mp2),
+			StartInput: sf.InputFromProtobuf(mp1),
+			EndInput:   sf.InputFromProtobuf(mp2),
 			Frame:      sf,
 		},
 		1,

diff --git a/motionplan/kinematic.go b/motionplan/kinematic.go
@@ -24,7 +24,7 @@ func ComputePosition(model referenceframe.Frame, joints *pb.JointPositions) (*co
 		)
 	}
 
-	pose, err := model.Transform(referenceframe.JointPosToInputs(joints))
+	pose, err := model.Transform(model.InputFromProtobuf(joints))
 	if err != nil {
 		return nil, err
 	}

diff --git a/motionplan/kinematic_test.go b/motionplan/kinematic_test.go
@@ -329,7 +329,7 @@ func TestSVAvsDH(t *testing.T) {
 
 	seed := rand.New(rand.NewSource(23))
 	for i := 0; i < numTests; i++ {
-		joints := frame.InputsToJointPos(frame.RandomFrameInputs(mSVA, seed))
+		joints := mSVA.ProtobufFromInput(frame.RandomFrameInputs(mSVA, seed))
 
 		posSVA, err := ComputePosition(mSVA, joints)
 		test.That(t, err, test.ShouldBeNil)

diff --git a/motionplan/nloptInverseKinematics_test.go b/motionplan/nloptInverseKinematics_test.go
@@ -29,7 +29,7 @@ func TestCreateNloptIKSolver(t *testing.T) {
 
 	pos = &commonpb.Pose{X: -46, Y: -23, Z: 372, Theta: utils.RadToDeg(3.92), OX: -0.46, OY: 0.84, OZ: 0.28}
 
-	seed = referenceframe.JointPosToInputs(&pb.JointPositions{Degrees: []float64{49, 28, -101, 0, -73, 0}})
+	seed = m.InputFromProtobuf(&pb.JointPositions{Degrees: []float64{49, 28, -101, 0, -73, 0}})
 
 	_, err = solveTest(context.Background(), ik, pos, seed)
 	test.That(t, err, test.ShouldBeNil)

diff --git a/motionplan/solvableFrameSystem.go b/motionplan/solvableFrameSystem.go
@@ -10,6 +10,7 @@ import (
 	"go.uber.org/multierr"
 
 	commonpb "go.viam.com/rdk/proto/api/common/v1"
+	pb "go.viam.com/rdk/proto/api/component/arm/v1"
 	frame "go.viam.com/rdk/referenceframe"
 	spatial "go.viam.com/rdk/spatialmath"
 )
@@ -170,6 +171,34 @@ func (sf *solverFrame) Transform(inputs []frame.Input) (spatial.Pose, error) {
 	return tf.(*frame.PoseInFrame).Pose(), nil
 }
 
+// InputFromProtobuf converts pb.JointPosition to inputs.
+func (sf *solverFrame) InputFromProtobuf(jp *pb.JointPositions) []frame.Input {
+	inputs := make([]frame.Input, 0, len(jp.Degrees))
+	posIdx := 0
+	for _, transform := range sf.frames {
+		dof := len(transform.DoF()) + posIdx
+		jPos := jp.Degrees[posIdx:dof]
+		posIdx = dof
+
+		inputs = append(inputs, transform.InputFromProtobuf(&pb.JointPositions{Degrees: jPos})...)
+	}
+
+	return inputs
+}
+
+// ProtobufFromInput converts inputs to pb.JointPosition.
+func (sf *solverFrame) ProtobufFromInput(input []frame.Input) *pb.JointPositions {
+	jPos := &pb.JointPositions{}
+	posIdx := 0
+	for _, transform := range sf.frames {
+		dof := len(transform.DoF()) + posIdx
+		jPos.Degrees = append(jPos.Degrees, transform.ProtobufFromInput(input[posIdx:dof]).Degrees...)
+		posIdx = dof
+	}
+
+	return jPos
+}
+
 // Geometry takes a solverFrame and a list of joint angles in radians and computes the 3D space occupied by each of the
 // geometries in the solverFrame in the reference frame of the World frame.
 func (sf *solverFrame) Geometries(inputs []frame.Input) (*frame.GeometriesInFrame, error) {