RSDK-9645 introduce consistent naming convention for frame system map…

… objects (#4664)

components/base/kinematicbase/execution.go

@@ -597,7 +597,7 @@ func (ptgk *ptgBaseKinematics) stepsToPlan(steps []arcStep, parentFrame string)
 	traj := motionplan.Trajectory{}
 	path := motionplan.Path{}
 	for _, step := range steps {
-		traj = append(traj, map[string][]referenceframe.Input{ptgk.Kinematics().Name(): step.arcSegment.EndConfiguration})
+		traj = append(traj, referenceframe.FrameSystemInputs{ptgk.Kinematics().Name(): step.arcSegment.EndConfiguration})
 		path = append(path, map[string]*referenceframe.PoseInFrame{
 			ptgk.Kinematics().Name(): referenceframe.NewPoseInFrame(parentFrame, step.arcSegment.EndPosition),
 		})

components/base/kinematicbase/ptgKinematics.go

@@ -206,7 +206,7 @@ func (ptgk *ptgBaseKinematics) ExecutionState(ctx context.Context) (motionplan.E
 	return motionplan.NewExecutionState(
 		currentPlan,
 		currentIdx,
-		map[string][]referenceframe.Input{ptgk.Kinematics().Name(): currentInputs},
+		referenceframe.FrameSystemInputs{ptgk.Kinematics().Name(): currentInputs},
 		map[string]*referenceframe.PoseInFrame{ptgk.LocalizationFrame().Name(): actualPIF},
 	)
 }

components/base/kinematicbase/ptgKinematics_test.go

@@ -75,10 +75,13 @@ func TestPTGKinematicsNoGeom(t *testing.T) {
 
 	test.That(t, err, test.ShouldBeNil)
 	fs.AddFrame(f, fs.World())
-	inputMap := referenceframe.StartPositions(fs)
+	inputMap := referenceframe.NewZeroInputs(fs)
 
-	startState := motionplan.NewPlanState(motionplan.PathState{f.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)}, inputMap)
-	goalState := motionplan.NewPlanState(motionplan.PathState{f.Name(): dstPIF}, nil)
+	startState := motionplan.NewPlanState(
+		referenceframe.FrameSystemPoses{f.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)},
+		inputMap,
+	)
+	goalState := motionplan.NewPlanState(referenceframe.FrameSystemPoses{f.Name(): dstPIF}, nil)
 
 	plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{
 		Logger:      logger,
@@ -153,7 +156,7 @@ func TestPTGKinematicsWithGeom(t *testing.T) {
 	f := kb.Kinematics()
 	test.That(t, err, test.ShouldBeNil)
 	fs.AddFrame(f, fs.World())
-	inputMap := referenceframe.StartPositions(fs)
+	inputMap := referenceframe.NewZeroInputs(fs)
 
 	obstacle, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2000, 0, 0}), r3.Vector{1, 1, 1}, "")
 	test.That(t, err, test.ShouldBeNil)
@@ -165,8 +168,11 @@ func TestPTGKinematicsWithGeom(t *testing.T) {
 	)
 	test.That(t, err, test.ShouldBeNil)
 
-	startState := motionplan.NewPlanState(motionplan.PathState{f.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)}, inputMap)
-	goalState := motionplan.NewPlanState(motionplan.PathState{f.Name(): dstPIF}, nil)
+	startState := motionplan.NewPlanState(
+		referenceframe.FrameSystemPoses{f.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)},
+		inputMap,
+	)
+	goalState := motionplan.NewPlanState(referenceframe.FrameSystemPoses{f.Name(): dstPIF}, nil)
 	plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{
 		Logger:      logger,
 		Goals:       []*motionplan.PlanState{goalState},

motionplan/cBiRRT.go

@@ -134,7 +134,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
 	defer cancel()
 	mp.start = time.Now()
 
-	var seed map[string][]referenceframe.Input
+	var seed referenceframe.FrameSystemInputs
 	// Pick a random (first in map) seed node to create the first interp node
 	for sNode, parent := range rrt.maps.startMap {
 		if parent == nil {
@@ -351,8 +351,8 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
 	ctx context.Context,
 	randseed *rand.Rand,
 	seedInputs,
-	target map[string][]referenceframe.Input,
-) map[string][]referenceframe.Input {
+	target referenceframe.FrameSystemInputs,
+) referenceframe.FrameSystemInputs {
 	for i := 0; i < maxNearIter; i++ {
 		select {
 		case <-ctx.Done():

motionplan/cBiRRT_test.go

@@ -39,19 +39,19 @@ func TestSimpleLinearMotion(t *testing.T) {
 	goalPos := spatialmath.NewPose(r3.Vector{X: 206, Y: 100, Z: 120.5}, &spatialmath.OrientationVectorDegrees{OY: -1})
 
 	opt := newBasicPlannerOptions()
-	goalMetric := opt.getGoalMetric(PathState{m.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos)})
+	goalMetric := opt.getGoalMetric(referenceframe.FrameSystemPoses{m.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos)})
 	fs := referenceframe.NewEmptyFrameSystem("")
 	fs.AddFrame(m, fs.World())
 	mp, err := newCBiRRTMotionPlanner(fs, rand.New(rand.NewSource(42)), logger, opt)
 	test.That(t, err, test.ShouldBeNil)
 	cbirrt, _ := mp.(*cBiRRTMotionPlanner)
-	solutions, err := mp.getSolutions(ctx, map[string][]referenceframe.Input{m.Name(): home7}, goalMetric)
+	solutions, err := mp.getSolutions(ctx, referenceframe.FrameSystemInputs{m.Name(): home7}, goalMetric)
 	test.That(t, err, test.ShouldBeNil)
 
-	near1 := &basicNode{q: map[string][]referenceframe.Input{m.Name(): home7}}
+	near1 := &basicNode{q: referenceframe.FrameSystemInputs{m.Name(): home7}}
 	seedMap := make(map[node]node)
 	seedMap[near1] = nil
-	target := map[string][]referenceframe.Input{m.Name(): interp}
+	target := referenceframe.FrameSystemInputs{m.Name(): interp}
 
 	goalMap := make(map[node]node)
 

motionplan/check.go

@@ -77,7 +77,7 @@ func checkPlanRelative(
 	sfPlanner *planManager,
 ) error {
 	var err error
-	toWorld := func(pif *referenceframe.PoseInFrame, inputs map[string][]referenceframe.Input) (*referenceframe.PoseInFrame, error) {
+	toWorld := func(pif *referenceframe.PoseInFrame, inputs referenceframe.FrameSystemInputs) (*referenceframe.PoseInFrame, error) {
 		transformable, err := fs.Transform(inputs, pif, referenceframe.World)
 		if err != nil {
 			return nil, err
@@ -331,7 +331,7 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDist
 		}
 		for _, interpConfig := range interpolatedConfigurations {
 			poseInPathTf, err := sfPlanner.fs.Transform(
-				map[string][]referenceframe.Input{checkFrame.Name(): interpConfig},
+				referenceframe.FrameSystemInputs{checkFrame.Name(): interpConfig},
 				referenceframe.NewZeroPoseInFrame(checkFrame.Name()),
 				parent.Name(),
 			)

motionplan/collision_test.go

@@ -6,7 +6,7 @@ import (
 	"github.com/golang/geo/r3"
 	"go.viam.com/test"
 
-	frame "go.viam.com/rdk/referenceframe"
+	"go.viam.com/rdk/referenceframe"
 	spatial "go.viam.com/rdk/spatialmath"
 	"go.viam.com/rdk/utils"
 )
@@ -73,21 +73,21 @@ func TestCheckCollisions(t *testing.T) {
 
 	// case 2: zero position of xArm6 arm - should have number of collisions = to number of geometries - 1
 	// no external geometries considered, self collision only
-	m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "")
+	m, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "")
 	test.That(t, err, test.ShouldBeNil)
-	gf, _ := m.Geometries(make([]frame.Input, len(m.DoF())))
+	gf, _ := m.Geometries(make([]referenceframe.Input, len(m.DoF())))
 	test.That(t, gf, test.ShouldNotBeNil)
 	cg, err = newCollisionGraph(gf.Geometries(), gf.Geometries(), nil, true, defaultCollisionBufferMM)
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, len(cg.collisions(defaultCollisionBufferMM)), test.ShouldEqual, 4)
 }
 
 func TestUniqueCollisions(t *testing.T) {
-	m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "")
+	m, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "")
 	test.That(t, err, test.ShouldBeNil)
 
 	// zero position of xarm6 arm
-	input := make([]frame.Input, len(m.DoF()))
+	input := make([]referenceframe.Input, len(m.DoF()))
 	internalGeometries, _ := m.Geometries(input)
 	test.That(t, internalGeometries, test.ShouldNotBeNil)
 	zeroPositionCG, err := newCollisionGraph(
@@ -100,7 +100,7 @@ func TestUniqueCollisions(t *testing.T) {
 	test.That(t, err, test.ShouldBeNil)
 
 	// case 1: no self collision - check no new collisions are returned
-	input[0] = frame.Input{Value: 1}
+	input[0] = referenceframe.Input{Value: 1}
 	internalGeometries, _ = m.Geometries(input)
 	test.That(t, internalGeometries, test.ShouldNotBeNil)
 	cg, err := newCollisionGraph(
@@ -114,7 +114,7 @@ func TestUniqueCollisions(t *testing.T) {
 	test.That(t, len(cg.collisions(defaultCollisionBufferMM)), test.ShouldEqual, 0)
 
 	// case 2: self collision - check only new collisions are returned
-	input[4] = frame.Input{Value: 2}
+	input[4] = referenceframe.Input{Value: 2}
 	internalGeometries, _ = m.Geometries(input)
 	test.That(t, internalGeometries, test.ShouldNotBeNil)
 	cg, err = newCollisionGraph(

motionplan/constraint.go

@@ -709,7 +709,7 @@ func (c *Constraints) GetCollisionSpecification() []CollisionSpecification {
 type fsPathConstraint struct {
 	metricMap     map[string]ik.StateMetric
 	constraintMap map[string]StateConstraint
-	goalMap       PathState
+	goalMap       referenceframe.FrameSystemPoses
 	fs            referenceframe.FrameSystem
 }
 
@@ -754,8 +754,8 @@ func (fpc *fsPathConstraint) metric(state *ik.StateFS) float64 {
 
 func newFsPathConstraintSeparatedLinOrientTol(
 	fs referenceframe.FrameSystem,
-	startCfg map[string][]referenceframe.Input,
-	from, to PathState,
+	startCfg referenceframe.FrameSystemInputs,
+	from, to referenceframe.FrameSystemPoses,
 	constructor func(spatial.Pose, spatial.Pose, float64, float64) (StateConstraint, ik.StateMetric),
 	linTol, orientTol float64,
 ) (*fsPathConstraint, error) {
@@ -786,8 +786,8 @@ func newFsPathConstraintSeparatedLinOrientTol(
 
 func newFsPathConstraintTol(
 	fs referenceframe.FrameSystem,
-	startCfg map[string][]referenceframe.Input,
-	from, to PathState,
+	startCfg referenceframe.FrameSystemInputs,
+	from, to referenceframe.FrameSystemPoses,
 	constructor func(spatial.Pose, spatial.Pose, float64) (StateConstraint, ik.StateMetric),
 	tolerance float64,
 ) (*fsPathConstraint, error) {
@@ -821,8 +821,8 @@ func newFsPathConstraintTol(
 // their respective orientations, as well as a metric which returns the distance to that valid region.
 func CreateSlerpOrientationConstraintFS(
 	fs referenceframe.FrameSystem,
-	startCfg map[string][]referenceframe.Input,
-	from, to PathState,
+	startCfg referenceframe.FrameSystemInputs,
+	from, to referenceframe.FrameSystemPoses,
 	tolerance float64,
 ) (StateFSConstraint, ik.StateFSMetric, error) {
 	constraintInternal, err := newFsPathConstraintTol(fs, startCfg, from, to, NewSlerpOrientationConstraint, tolerance)
@@ -837,8 +837,8 @@ func CreateSlerpOrientationConstraintFS(
 // line segment between their respective positions, as well as a metric which returns the distance to that valid region.
 func CreateLineConstraintFS(
 	fs referenceframe.FrameSystem,
-	startCfg map[string][]referenceframe.Input,
-	from, to PathState,
+	startCfg referenceframe.FrameSystemInputs,
+	from, to referenceframe.FrameSystemPoses,
 	tolerance float64,
 ) (StateFSConstraint, ik.StateFSMetric, error) {
 	// Need to define a constructor here since NewLineConstraint takes r3.Vectors, not poses
@@ -857,8 +857,8 @@ func CreateLineConstraintFS(
 // orientation deviation measured by norm of the R3AA orientation difference to the slerp path between start/goal orientations.
 func CreateAbsoluteLinearInterpolatingConstraintFS(
 	fs referenceframe.FrameSystem,
-	startCfg map[string][]referenceframe.Input,
-	from, to PathState,
+	startCfg referenceframe.FrameSystemInputs,
+	from, to referenceframe.FrameSystemPoses,
 	linTol, orientTol float64,
 ) (StateFSConstraint, ik.StateFSMetric, error) {
 	constraintInternal, err := newFsPathConstraintSeparatedLinOrientTol(
@@ -881,8 +881,8 @@ func CreateAbsoluteLinearInterpolatingConstraintFS(
 // from start to goal.
 func CreateProportionalLinearInterpolatingConstraintFS(
 	fs referenceframe.FrameSystem,
-	startCfg map[string][]referenceframe.Input,
-	from, to PathState,
+	startCfg referenceframe.FrameSystemInputs,
+	from, to referenceframe.FrameSystemPoses,
 	linTol, orientTol float64,
 ) (StateFSConstraint, ik.StateFSMetric, error) {
 	constraintInternal, err := newFsPathConstraintSeparatedLinOrientTol(

motionplan/constraint_handler.go

@@ -113,7 +113,7 @@ func interpolateSegment(ci *ik.Segment, resolution float64) ([][]referenceframe.
 		return nil, err
 	}
 
-	steps := PathStateCount(ci.StartPosition, ci.EndPosition, resolution)
+	steps := CalculateStepCount(ci.StartPosition, ci.EndPosition, resolution)
 	if steps < defaultMinStepCount {
 		// Minimum step count ensures we are not missing anything
 		steps = defaultMinStepCount
@@ -133,7 +133,7 @@ func interpolateSegment(ci *ik.Segment, resolution float64) ([][]referenceframe.
 
 // interpolateSegmentFS is a helper function which produces a list of intermediate inputs, between the start and end
 // configuration of a segment at a given resolution value.
-func interpolateSegmentFS(ci *ik.SegmentFS, resolution float64) ([]map[string][]referenceframe.Input, error) {
+func interpolateSegmentFS(ci *ik.SegmentFS, resolution float64) ([]referenceframe.FrameSystemInputs, error) {
 	// Find the frame with the most steps by calculating steps for each frame
 	maxSteps := defaultMinStepCount
 	for frameName, startConfig := range ci.StartConfiguration {
@@ -163,17 +163,17 @@ func interpolateSegmentFS(ci *ik.SegmentFS, resolution float64) ([]map[string][]
 		}
 
 		// Calculate steps needed for this frame
-		steps := PathStateCount(startPos, endPos, resolution)
+		steps := CalculateStepCount(startPos, endPos, resolution)
 		if steps > maxSteps {
 			maxSteps = steps
 		}
 	}
 
 	// Create interpolated configurations for all frames
-	var interpolatedConfigurations []map[string][]referenceframe.Input
+	var interpolatedConfigurations []referenceframe.FrameSystemInputs
 	for i := 0; i <= maxSteps; i++ {
 		interp := float64(i) / float64(maxSteps)
-		frameConfigs := make(map[string][]referenceframe.Input)
+		frameConfigs := make(referenceframe.FrameSystemInputs)
 
 		// Interpolate each frame's configuration
 		for frameName, startConfig := range ci.StartConfiguration {
@@ -318,7 +318,7 @@ func (c *ConstraintHandler) CheckStateConstraintsAcrossSegmentFS(ci *ik.SegmentF
 	if err != nil {
 		return false, nil
 	}
-	var lastGood map[string][]referenceframe.Input
+	var lastGood referenceframe.FrameSystemInputs
 	for i, interpConfig := range interpolatedConfigurations {
 		interpC := &ik.StateFS{FS: ci.FS, Configuration: interpConfig}
 		pass, _ := c.CheckStateFSConstraints(interpC)

motionplan/constraint_test.go

@@ -29,14 +29,10 @@ func TestIKTolerances(t *testing.T) {
 	test.That(t, err, test.ShouldBeNil)
 
 	// Test inability to arrive at another position due to orientation
-	goal := &PlanState{poses: PathState{m.Name(): frame.NewPoseInFrame(frame.World, spatial.NewPoseFromProtobuf(&commonpb.Pose{
-		X:  -46,
-		Y:  0,
-		Z:  372,
-		OX: -1.78,
-		OY: -3.3,
-		OZ: -1.11,
-	}))}}
+	goal := &PlanState{poses: frame.FrameSystemPoses{m.Name(): frame.NewPoseInFrame(
+		frame.World,
+		spatial.NewPoseFromProtobuf(&commonpb.Pose{X: -46, Y: 0, Z: 372, OX: -1.78, OY: -3.3, OZ: -1.11}),
+	)}}
 	seed := &PlanState{configuration: map[string][]frame.Input{m.Name(): frame.FloatsToInputs(make([]float64, 6))}}
 	_, err = mp.plan(context.Background(), seed, goal)
 	test.That(t, err, test.ShouldNotBeNil)
@@ -151,8 +147,8 @@ func TestLineFollow(t *testing.T) {
 
 	opt := newBasicPlannerOptions()
 	startCfg := map[string][]frame.Input{m.Name(): m.InputFromProtobuf(mp1)}
-	from := PathState{markerFrame.Name(): frame.NewPoseInFrame(markerFrame.Name(), p1)}
-	to := PathState{markerFrame.Name(): frame.NewPoseInFrame(goalFrame.Name(), p2)}
+	from := frame.FrameSystemPoses{markerFrame.Name(): frame.NewPoseInFrame(markerFrame.Name(), p1)}
+	to := frame.FrameSystemPoses{markerFrame.Name(): frame.NewPoseInFrame(goalFrame.Name(), p2)}
 
 	validFunc, gradFunc, err := CreateLineConstraintFS(fs, startCfg, from, to, 0.001)
 	test.That(t, err, test.ShouldBeNil)
@@ -220,7 +216,7 @@ func TestCollisionConstraints(t *testing.T) {
 	fs := frame.NewEmptyFrameSystem("test")
 	err = fs.AddFrame(model, fs.Frame(frame.World))
 	test.That(t, err, test.ShouldBeNil)
-	seedMap := frame.StartPositions(fs)
+	seedMap := frame.NewZeroInputs(fs)
 	handler := &ConstraintHandler{}
 
 	// create robot collision entities
@@ -284,7 +280,7 @@ func BenchmarkCollisionConstraints(b *testing.B) {
 	fs := frame.NewEmptyFrameSystem("test")
 	err = fs.AddFrame(model, fs.Frame(frame.World))
 	test.That(b, err, test.ShouldBeNil)
-	seedMap := frame.StartPositions(fs)
+	seedMap := frame.NewZeroInputs(fs)
 	handler := &ConstraintHandler{}
 
 	// create robot collision entities

motionplan/ik/metrics.go

@@ -25,8 +25,8 @@ type Segment struct {
 // SegmentFS is a referenceframe.FrameSystem-specific contains all the information a constraint needs to determine validity for a movement.
 // It contains the starting inputs, the ending inputs, and the framesystem it refers to.
 type SegmentFS struct {
-	StartConfiguration map[string][]referenceframe.Input
-	EndConfiguration   map[string][]referenceframe.Input
+	StartConfiguration referenceframe.FrameSystemInputs
+	EndConfiguration   referenceframe.FrameSystemInputs
 	FS                 referenceframe.FrameSystem
 }
 
@@ -62,7 +62,7 @@ type State struct {
 // framesystem. It contains inputs, the corresponding poses, and the frame it refers to.
 // Pose field may be empty, and may be filled in by a constraint that needs it.
 type StateFS struct {
-	Configuration map[string][]referenceframe.Input
+	Configuration referenceframe.FrameSystemInputs
 	FS            referenceframe.FrameSystem
 }
 

motionplan/kinematic_test.go

@@ -163,7 +163,7 @@ func TestDynamicFrameSystemXArm(t *testing.T) {
 	test.That(t, err, test.ShouldBeNil)
 	fs.AddFrame(model, fs.World())
 
-	positions := frame.StartPositions(fs)
+	positions := frame.NewZeroInputs(fs)
 
 	// World point of xArm at 0 position
 	poseWorld1 := spatial.NewPoseFromPoint(r3.Vector{207, 0, 112})
@@ -227,7 +227,7 @@ func TestComplicatedDynamicFrameSystem(t *testing.T) {
 	test.That(t, err, test.ShouldBeNil)
 	fs.AddFrame(urCamera, modelUR5e)
 
-	positions := frame.StartPositions(fs)
+	positions := frame.NewZeroInputs(fs)
 
 	poseUR5e := spatial.NewPoseFromPoint(r3.Vector{-717.2, -132.9, 262.8})
 	// Camera translates by 30, gripper is pointed at -Y