Create type equivalence between referenceframe.Input and float64s (#5391)

Co-authored-by: Dan Gottlieb <[email protected]>

Author: erh

components/arm/arm.go

@@ -187,17 +187,17 @@ func CheckDesiredJointPositions(ctx context.Context, a Arm, desiredInputs []refe
 		min := limits[i].Min
 		currPosition := currentJointPos[i]
 		// to make sure that val is a valid input it must either bring the joint closer inbounds or keep the joint inbounds.
-		if currPosition.Value > limits[i].Max {
-			max = currPosition.Value
-		} else if currPosition.Value < limits[i].Min {
-			min = currPosition.Value
+		if currPosition > limits[i].Max {
+			max = currPosition
+		} else if currPosition < limits[i].Min {
+			min = currPosition
 		}
-		if val.Value > max || val.Value < min {
+		if val > max || val < min {
 			return fmt.Errorf("joint %v needs to be within range [%v, %v] and cannot be moved to %v",
 				i,
 				utils.RadToDeg(min),
 				utils.RadToDeg(max),
-				utils.RadToDeg(val.Value),
+				utils.RadToDeg(val),
 			)
 		}
 	}

components/arm/client_test.go

@@ -44,7 +44,7 @@ func TestClient(t *testing.T) {
 	)
 
 	pos1 := spatialmath.NewPoseFromPoint(r3.Vector{X: 1, Y: 2, Z: 3})
-	jointPos1 := []referenceframe.Input{{1.}, {2.}, {3.}}
+	jointPos1 := []referenceframe.Input{1., 2., 3.}
 	expectedGeometries := []spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{1, 2, 3}, "")}
 	expectedMoveOptions := arm.MoveOptions{MaxVelRads: 1, MaxAccRads: 2}
 	injectArm := &inject.Arm{}
@@ -89,7 +89,7 @@ func TestClient(t *testing.T) {
 	}
 
 	pos2 := spatialmath.NewPoseFromPoint(r3.Vector{X: 4, Y: 5, Z: 6})
-	jointPos2 := []referenceframe.Input{{4.}, {5.}, {6.}}
+	jointPos2 := []referenceframe.Input{4., 5., 6.}
 	injectArm2 := &inject.Arm{}
 	injectArm2.EndPositionFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) {
 		return pos2, nil

components/arm/collectors_test.go

@@ -117,7 +117,7 @@ func newArm() arm.Arm {
 		return spatialmath.NewPoseFromPoint(r3.Vector{X: 1, Y: 2, Z: 3}), nil
 	}
 	a.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) {
-		return referenceframe.FloatsToInputs(referenceframe.JointPositionsToRadians(floatList)), nil
+		return referenceframe.JointPositionsToRadians(floatList), nil
 	}
 	a.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
 		return nil, nil

components/arm/fake/fake.go

@@ -137,7 +137,7 @@ func (a *Arm) Reconfigure(ctx context.Context, deps resource.Dependencies, conf
 
 	a.mu.Lock()
 	defer a.mu.Unlock()
-	a.joints = referenceframe.FloatsToInputs(make([]float64, dof))
+	a.joints = make([]referenceframe.Input, dof)
 	a.model = model
 
 	return nil

components/arm/fake/fake_test.go

@@ -94,7 +94,7 @@ func TestJointPositions(t *testing.T) {
 	// Round trip test for MoveToJointPositions -> JointPositions
 	arm, err := NewArm(ctx, nil, cfg, logger)
 	test.That(t, err, test.ShouldBeNil)
-	samplePositions := []referenceframe.Input{{0}, {math.Pi}, {-math.Pi}, {0}, {math.Pi}, {-math.Pi}}
+	samplePositions := []referenceframe.Input{0, math.Pi, -math.Pi, 0, math.Pi, -math.Pi}
 	test.That(t, arm.MoveToJointPositions(ctx, samplePositions, nil), test.ShouldBeNil)
 	positions, err := arm.JointPositions(ctx, nil)
 	test.That(t, err, test.ShouldBeNil)

components/arm/server.go

@@ -180,7 +180,7 @@ func (s *serviceServer) GetGeometries(ctx context.Context, req *commonpb.GetGeom
 			}
 			jointPositionsPb := jointPbResp.GetPositions()
 
-			gifs, err := model.Geometries(model.InputFromProtobuf(jointPositionsPb))
+			gifs, err := model.Geometries(jointPositionsPb.Values)
 			if err != nil {
 				return nil, err
 			}

components/arm/server_test.go

@@ -70,7 +70,7 @@ func TestServer(t *testing.T) {
 	}
 	injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) {
 		extraOptions = extra
-		return referenceframe.FloatsToInputs(positions), nil
+		return positions, nil
 	}
 	injectArm.MoveToPositionFunc = func(ctx context.Context, ap spatialmath.Pose, extra map[string]interface{}) error {
 		capArmPos = ap

components/base/kinematicbase/differentialDrive.go

@@ -33,7 +33,7 @@ var (
 	// errMovementTimeout is used for when a movement call times out after no movement for some time.
 	errMovementTimeout = errors.New("movement has timed out")
 	// Input representation of origin.
-	originInputs = []referenceframe.Input{{Value: 0}, {Value: 0}, {Value: 0}}
+	originInputs = []referenceframe.Input{0, 0, 0}
 )
 
 // wrapWithDifferentialDriveKinematics takes a wheeledBase component and adds a localizer to it
@@ -141,7 +141,7 @@ func (ddk *differentialDriveKinematics) CurrentInputs(ctx context.Context) ([]re
 	// We should not have a problem with Gimbal lock by looking at yaw in the domain that most bases will be moving.
 	// This could potentially be made more robust in the future, though.
 	theta := math.Mod(pif.Pose().Orientation().EulerAngles().Yaw, 2*math.Pi)
-	return []referenceframe.Input{{Value: pt.X}, {Value: pt.Y}, {Value: theta}}, nil
+	return []referenceframe.Input{pt.X, pt.Y, theta}, nil
 }
 
 func (ddk *differentialDriveKinematics) GoToInputs(ctx context.Context, desiredSteps ...[]referenceframe.Input) error {
@@ -191,7 +191,7 @@ func (ddk *differentialDriveKinematics) goToInputs(ctx context.Context, desired
 		// when the base is within the positional threshold of the goal, exit the loop
 		for err := cancelContext.Err(); err == nil; err = cancelContext.Err() {
 			utils.SelectContextOrWait(ctx, 10*time.Millisecond)
-			point := spatialmath.NewPoint(r3.Vector{X: current[0].Value, Y: current[1].Value}, "")
+			point := spatialmath.NewPoint(r3.Vector{X: current[0], Y: current[1]}, "")
 			col, err := validRegion.CollidesWith(point, defaultCollisionBufferMM)
 			if err != nil {
 				movementErr <- err
@@ -203,8 +203,8 @@ func (ddk *differentialDriveKinematics) goToInputs(ctx context.Context, desired
 			}
 
 			// get to the x, y location first - note that from the base's perspective +y is forward
-			desiredHeading := math.Atan2(desired[1].Value-current[1].Value, desired[0].Value-current[0].Value)
-			commanded, err := ddk.issueCommand(cancelContext, current, []referenceframe.Input{desired[0], desired[1], {Value: desiredHeading}})
+			desiredHeading := math.Atan2(desired[1]-current[1], desired[0]-current[0])
+			commanded, err := ddk.issueCommand(cancelContext, current, []referenceframe.Input{desired[0], desired[1], desiredHeading})
 			if err != nil {
 				movementErr <- err
 				return
@@ -290,7 +290,7 @@ func (ddk *differentialDriveKinematics) issueCommand(ctx context.Context, curren
 		if ddk.Localizer == nil {
 			ddk.mutex.Lock()
 			defer ddk.mutex.Unlock()
-			ddk.noLocalizerCacheInputs = []referenceframe.Input{{Value: 0}, {Value: 0}, desired[2]}
+			ddk.noLocalizerCacheInputs = []referenceframe.Input{0, 0, desired[2]}
 			time.Sleep(defaultNoLocalizerDelay)
 		}
 		return true, err
@@ -314,8 +314,8 @@ func (ddk *differentialDriveKinematics) issueCommand(ctx context.Context, curren
 func (ddk *differentialDriveKinematics) inputDiff(current, desired []referenceframe.Input) (float64, float64, error) {
 	// create a goal pose in the world frame
 	goal := spatialmath.NewPose(
-		r3.Vector{X: desired[0].Value, Y: desired[1].Value},
-		&spatialmath.OrientationVector{OZ: 1, Theta: desired[2].Value},
+		r3.Vector{X: desired[0], Y: desired[1]},
+		&spatialmath.OrientationVector{OZ: 1, Theta: desired[2]},
 	)
 
 	// transform the goal pose such that it is in the base frame
@@ -336,13 +336,13 @@ func (ddk *differentialDriveKinematics) inputDiff(current, desired []referencefr
 // starting and ending waypoints. This capsule is used to detect whether a base leaves this region and has thus deviated
 // too far from its path.
 func (ddk *differentialDriveKinematics) newValidRegionCapsule(starting, desired []referenceframe.Input) (spatialmath.Geometry, error) {
-	pt := r3.Vector{X: (desired[0].Value + starting[0].Value) / 2, Y: (desired[1].Value + starting[1].Value) / 2}
-	positionErr, _, err := ddk.inputDiff(starting, []referenceframe.Input{desired[0], desired[1], {Value: 0}})
+	pt := r3.Vector{X: (desired[0] + starting[0]) / 2, Y: (desired[1] + starting[1]) / 2}
+	positionErr, _, err := ddk.inputDiff(starting, []referenceframe.Input{desired[0], desired[1], 0})
 	if err != nil {
 		return nil, err
 	}
 
-	desiredHeading := math.Atan2(starting[0].Value-desired[0].Value, starting[1].Value-desired[1].Value)
+	desiredHeading := math.Atan2(starting[0]-desired[0], starting[1]-desired[1])
 
 	// rotate such that y is forward direction to match the frame for movement of a base
 	// rotate around the z-axis such that the capsule points in the direction of the end waypoint

components/base/kinematicbase/differentialDrive_test.go

@@ -124,7 +124,7 @@ func TestCurrentInputs(t *testing.T) {
 		for i := 0; i < 10; i++ {
 			input, err := ddk.CurrentInputs(ctx)
 			test.That(t, err, test.ShouldBeNil)
-			test.That(t, input, test.ShouldResemble, []referenceframe.Input{{Value: 0}, {Value: 0}, {Value: 0}})
+			test.That(t, input, test.ShouldResemble, []referenceframe.Input{0, 0, 0})
 		}
 	})
 }
@@ -145,10 +145,10 @@ func TestInputDiff(t *testing.T) {
 	test.That(t, err, test.ShouldBeNil)
 	ddk.Localizer = motion.NewSLAMLocalizer(slam)
 
-	desiredInput := []referenceframe.Input{{Value: 3}, {Value: 4}, {Value: utils.DegToRad(30)}}
+	desiredInput := []referenceframe.Input{3, 4, utils.DegToRad(30)}
 	distErr, headingErr, err := ddk.inputDiff(make([]referenceframe.Input, 3), desiredInput)
 	test.That(t, err, test.ShouldBeNil)
-	test.That(t, distErr, test.ShouldAlmostEqual, r3.Vector{X: desiredInput[0].Value, Y: desiredInput[1].Value, Z: 0}.Norm())
+	test.That(t, distErr, test.ShouldAlmostEqual, r3.Vector{X: desiredInput[0], Y: desiredInput[1], Z: 0}.Norm())
 	test.That(t, headingErr, test.ShouldAlmostEqual, 30)
 }
 
@@ -204,8 +204,8 @@ func TestNewValidRegionCapsule(t *testing.T) {
 	ddk, err := buildTestDDK(ctx, testConfig(), true, defaultLinearVelocityMMPerSec, defaultAngularVelocityDegsPerSec, logger)
 	test.That(t, err, test.ShouldBeNil)
 
-	starting := referenceframe.FloatsToInputs([]float64{400, 0, 0})
-	desired := referenceframe.FloatsToInputs([]float64{0, 400, 0})
+	starting := []referenceframe.Input{400, 0, 0}
+	desired := []referenceframe.Input{0, 400, 0}
 	c, err := ddk.newValidRegionCapsule(starting, desired)
 	test.That(t, err, test.ShouldBeNil)
 

components/base/kinematicbase/execution.go

@@ -164,14 +164,14 @@ func (ptgk *ptgBaseKinematics) GoToInputs(ctx context.Context, inputSteps ...[]r
 					return tryStop(ctx.Err())
 				}
 			}
-			inputValDiff := step.arcSegment.EndConfiguration[endDistanceAlongTrajectoryIndex].Value -
-				step.arcSegment.EndConfiguration[startDistanceAlongTrajectoryIndex].Value
+			inputValDiff := step.arcSegment.EndConfiguration[endDistanceAlongTrajectoryIndex] -
+				step.arcSegment.EndConfiguration[startDistanceAlongTrajectoryIndex]
 			elapsedPct := math.Min(1.0, timeElapsedSeconds/step.durationSeconds)
 			currentInputs := []referenceframe.Input{
 				step.arcSegment.StartConfiguration[ptgIndex],
 				step.arcSegment.StartConfiguration[trajectoryAlphaWithinPTG],
 				step.arcSegment.StartConfiguration[startDistanceAlongTrajectoryIndex],
-				{step.arcSegment.StartConfiguration[startDistanceAlongTrajectoryIndex].Value + inputValDiff*elapsedPct},
+				step.arcSegment.StartConfiguration[startDistanceAlongTrajectoryIndex] + inputValDiff*elapsedPct,
 			}
 			ptgk.inputLock.Lock()
 			ptgk.currentState.currentInputs = currentInputs
@@ -231,12 +231,12 @@ func (ptgk *ptgBaseKinematics) trajectoryArcSteps(
 	startPose spatialmath.Pose,
 	inputs []referenceframe.Input,
 ) ([]arcStep, error) {
-	selectedPTG := int(math.Round(inputs[ptgIndex].Value))
+	selectedPTG := int(math.Round(inputs[ptgIndex]))
 
 	traj, err := ptgk.ptgs[selectedPTG].Trajectory(
-		inputs[trajectoryAlphaWithinPTG].Value,
-		inputs[startDistanceAlongTrajectoryIndex].Value,
-		inputs[endDistanceAlongTrajectoryIndex].Value,
+		inputs[trajectoryAlphaWithinPTG],
+		inputs[startDistanceAlongTrajectoryIndex],
+		inputs[endDistanceAlongTrajectoryIndex],
 		stepDistResolution,
 	)
 	if err != nil {
@@ -245,7 +245,7 @@ func (ptgk *ptgBaseKinematics) trajectoryArcSteps(
 
 	finalSteps := []arcStep{}
 	timeStep := 0.
-	curDist := inputs[startDistanceAlongTrajectoryIndex].Value
+	curDist := inputs[startDistanceAlongTrajectoryIndex]
 	startInputs := []referenceframe.Input{
 		inputs[ptgIndex],
 		inputs[trajectoryAlphaWithinPTG],
@@ -289,7 +289,7 @@ func (ptgk *ptgBaseKinematics) trajectoryArcSteps(
 				inputs[ptgIndex],
 				inputs[trajectoryAlphaWithinPTG],
 				nextStep.arcSegment.StartConfiguration[startDistanceAlongTrajectoryIndex],
-				{curDist},
+				curDist,
 			}
 			nextStep.arcSegment.EndConfiguration = stepEndInputs
 
@@ -304,8 +304,8 @@ func (ptgk *ptgBaseKinematics) trajectoryArcSteps(
 			stepStartInputs := []referenceframe.Input{
 				inputs[ptgIndex],
 				inputs[trajectoryAlphaWithinPTG],
-				{curDist},
-				{curDist},
+				curDist,
+				curDist,
 			}
 			segment = motionplan.Segment{
 				StartConfiguration: stepStartInputs,
@@ -328,7 +328,7 @@ func (ptgk *ptgBaseKinematics) trajectoryArcSteps(
 		inputs[ptgIndex],
 		inputs[trajectoryAlphaWithinPTG],
 		nextStep.arcSegment.StartConfiguration[startDistanceAlongTrajectoryIndex],
-		{curDist},
+		curDist,
 	}
 	nextStep.arcSegment.EndConfiguration = finalInputs
 	arcPose, err := ptgk.planningModel.Transform(finalInputs)
@@ -399,7 +399,7 @@ func (ptgk *ptgBaseKinematics) courseCorrect(
 				// We've got a course correction solution. Swap out the relevant arcsteps.
 				newArcSteps, err := ptgk.trajectoryArcSteps(
 					actualPoseTracked,
-					[]referenceframe.Input{{float64(ptgk.courseCorrectionIdx)}, solution.Solution[i], {0}, solution.Solution[i+1]},
+					[]referenceframe.Input{float64(ptgk.courseCorrectionIdx), solution.Solution[i], 0, solution.Solution[i+1]},
 				)
 				if err != nil {
 					return nil, err
@@ -422,8 +422,8 @@ func (ptgk *ptgBaseKinematics) courseCorrect(
 			connectionPointDeepCopy := copyArcStep(connectionPoint)
 
 			arcOriginalLength := math.Abs(
-				connectionPointDeepCopy.arcSegment.EndConfiguration[endDistanceAlongTrajectoryIndex].Value -
-					connectionPointDeepCopy.arcSegment.EndConfiguration[startDistanceAlongTrajectoryIndex].Value,
+				connectionPointDeepCopy.arcSegment.EndConfiguration[endDistanceAlongTrajectoryIndex] -
+					connectionPointDeepCopy.arcSegment.EndConfiguration[startDistanceAlongTrajectoryIndex],
 			)
 
 			// Use distances to calculate the % completion of the arc, used to update the time remaining.
@@ -444,24 +444,24 @@ func (ptgk *ptgBaseKinematics) courseCorrect(
 				connectionPointDeepCopy.arcSegment.EndConfiguration[ptgIndex],
 				connectionPointDeepCopy.arcSegment.EndConfiguration[trajectoryAlphaWithinPTG],
 				connectionPointDeepCopy.arcSegment.EndConfiguration[startDistanceAlongTrajectoryIndex],
-				{startVal},
+				startVal,
 			}
 			skippedPose, err := ptgk.planningModel.Transform(skippedSegment)
 			if err != nil {
 				return nil, err
 			}
 
-			isReverse := connectionPointDeepCopy.arcSegment.EndConfiguration[endDistanceAlongTrajectoryIndex].Value < 0
+			isReverse := connectionPointDeepCopy.arcSegment.EndConfiguration[endDistanceAlongTrajectoryIndex] < 0
 			if isReverse {
-				startVal += connectionPointDeepCopy.arcSegment.EndConfiguration[endDistanceAlongTrajectoryIndex].Value
+				startVal += connectionPointDeepCopy.arcSegment.EndConfiguration[endDistanceAlongTrajectoryIndex]
 			}
 
-			connectionPointDeepCopy.arcSegment.StartConfiguration[startDistanceAlongTrajectoryIndex].Value = startVal
-			connectionPointDeepCopy.arcSegment.StartConfiguration[endDistanceAlongTrajectoryIndex].Value = startVal
+			connectionPointDeepCopy.arcSegment.StartConfiguration[startDistanceAlongTrajectoryIndex] = startVal
+			connectionPointDeepCopy.arcSegment.StartConfiguration[endDistanceAlongTrajectoryIndex] = startVal
 			if isReverse {
-				connectionPointDeepCopy.arcSegment.EndConfiguration[endDistanceAlongTrajectoryIndex].Value = startVal
+				connectionPointDeepCopy.arcSegment.EndConfiguration[endDistanceAlongTrajectoryIndex] = startVal
 			} else {
-				connectionPointDeepCopy.arcSegment.EndConfiguration[startDistanceAlongTrajectoryIndex].Value = startVal
+				connectionPointDeepCopy.arcSegment.EndConfiguration[startDistanceAlongTrajectoryIndex] = startVal
 			}
 			// The start position should be where the connection tried to get to.
 			// This needs to be the Goal, as that is the point along the original path, not the solved point, which is just somewhere near
@@ -497,11 +497,11 @@ func (ptgk *ptgBaseKinematics) getCorrectionSolution(ctx context.Context, goals
 	for _, goal := range goals {
 		solveMetric := motionplan.NewScaledSquaredNormMetric(goal.Goal, 50)
 		ptgk.logger.Debug("attempting goal", goal.Goal)
-		seed := []referenceframe.Input{{math.Pi / 2}, {ptgk.linVelocityMMPerSecond / 2}, {math.Pi / 2}, {ptgk.linVelocityMMPerSecond / 2}}
+		seed := []referenceframe.Input{math.Pi / 2, ptgk.linVelocityMMPerSecond / 2, math.Pi / 2, ptgk.linVelocityMMPerSecond / 2}
 		if goal.Goal.Point().X > 0 {
-			seed[0].Value *= -1
+			seed[0] *= -1
 		} else {
-			seed[2].Value *= -1
+			seed[2] *= -1
 		}
 		// Attempt to use our course correction solver to solve for a new set of trajectories which will get us from our current position
 		// to our goal point along our original trajectory.
@@ -515,7 +515,7 @@ func (ptgk *ptgBaseKinematics) getCorrectionSolution(ctx context.Context, goals
 		}
 		ptgk.logger.Debug("solution", solution)
 		if solution.Score < courseCorrectionMaxScore {
-			goal.Solution = referenceframe.FloatsToInputs(solution.Configuration)
+			goal.Solution = solution.Configuration
 			return goal, nil
 		}
 	}
@@ -531,7 +531,7 @@ func (ptgk *ptgBaseKinematics) makeCourseCorrectionGoals(
 	currentInputs []referenceframe.Input,
 ) []courseCorrectionGoal {
 	goals := []courseCorrectionGoal{}
-	currDist := currentInputs[endDistanceAlongTrajectoryIndex].Value
+	currDist := currentInputs[endDistanceAlongTrajectoryIndex]
 	stepsPerGoal := int((ptgk.nonzeroBaseTurningRadiusMeters*lookaheadDistMult*1000)/stepDistResolution) / nGoals
 
 	if stepsPerGoal < 1 {
@@ -568,7 +568,7 @@ func (ptgk *ptgBaseKinematics) makeCourseCorrectionGoals(
 				steps[i].arcSegment.StartConfiguration[ptgIndex],
 				steps[i].arcSegment.StartConfiguration[trajectoryAlphaWithinPTG],
 				steps[i].arcSegment.StartConfiguration[startDistanceAlongTrajectoryIndex],
-				{steps[i].subTraj[goalTrajPtIdx].Dist},
+				steps[i].subTraj[goalTrajPtIdx].Dist,
 			}
 
 			arcPose, err := ptgk.planningModel.Transform(arcTrajInputs)