RSDK-5391 - Create distinct planning and execution frames for PTG KinematicWrapper

components/base/kinematicbase/differentialDrive.go

@@ -78,7 +78,7 @@ func wrapWithDifferentialDriveKinematics(
 		}
 	}
 
-	ddk.executionFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, boundingSphere)
+	ddk.localizationFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, boundingSphere)
 	if err != nil {
 		return nil, err
 	}
@@ -89,7 +89,7 @@ func wrapWithDifferentialDriveKinematics(
 			return nil, err
 		}
 	} else {
-		ddk.planningFrame = ddk.executionFrame
+		ddk.planningFrame = ddk.localizationFrame
 	}
 
 	ddk.noLocalizerCacheInputs = originInputs
@@ -99,19 +99,23 @@ func wrapWithDifferentialDriveKinematics(
 type differentialDriveKinematics struct {
 	base.Base
 	motion.Localizer
-	logger                        logging.Logger
-	planningFrame, executionFrame referenceframe.Model
-	options                       Options
-	noLocalizerCacheInputs        []referenceframe.Input
-	currentTrajectory             [][]referenceframe.Input
-	currentIdx                    int
-	mutex                         sync.RWMutex
+	logger                           logging.Logger
+	planningFrame, localizationFrame referenceframe.Frame
+	options                          Options
+	noLocalizerCacheInputs           []referenceframe.Input
+	currentTrajectory                [][]referenceframe.Input
+	currentIdx                       int
+	mutex                            sync.RWMutex
 }
 
 func (ddk *differentialDriveKinematics) Kinematics() referenceframe.Frame {
 	return ddk.planningFrame
 }
 
+func (ddk *differentialDriveKinematics) LocalizationFrame() referenceframe.Frame {
+	return ddk.localizationFrame
+}
+
 func (ddk *differentialDriveKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) {
 	// If no localizer is present, CurrentInputs returns the expected position of the robot assuming after
 	// each part of move command was completed accurately.
@@ -310,7 +314,7 @@ func (ddk *differentialDriveKinematics) inputDiff(current, desired []referencefr
 	)
 
 	// transform the goal pose such that it is in the base frame
-	currentPose, err := ddk.executionFrame.Transform(current)
+	currentPose, err := ddk.localizationFrame.Transform(current)
 	if err != nil {
 		return 0, 0, err
 	}

components/base/kinematicbase/differentialDrive_test.go

@@ -68,12 +68,12 @@ func TestWrapWithDifferentialDriveKinematics(t *testing.T) {
 			if err != nil {
 				return
 			}
-			limits := ddk.executionFrame.DoF()
+			limits := ddk.localizationFrame.DoF()
 			test.That(t, limits[0].Min, test.ShouldBeLessThan, 0)
 			test.That(t, limits[1].Min, test.ShouldBeLessThan, 0)
 			test.That(t, limits[0].Max, test.ShouldBeGreaterThan, 0)
 			test.That(t, limits[1].Max, test.ShouldBeGreaterThan, 0)
-			geometry, err := ddk.executionFrame.(*referenceframe.SimpleModel).Geometries(make([]referenceframe.Input, len(limits)))
+			geometry, err := ddk.localizationFrame.(*referenceframe.SimpleModel).Geometries(make([]referenceframe.Input, len(limits)))
 			test.That(t, err, test.ShouldBeNil)
 			equivalent := spatialmath.GeometriesAlmostEqual(
 				geometry.GeometryByName(testCfg.Name+":"+testCfg.Frame.Geometry.Label),

components/base/kinematicbase/execution.go

@@ -337,7 +337,7 @@ func (ptgk *ptgBaseKinematics) courseCorrect(
 		return nil, err
 	}
 	// trajPose is the pose we should have nominally reached along the currently executing arc from the start position.
-	trajPose, err := ptgk.frame.Transform(currentInputs)
+	trajPose, err := ptgk.Kinematics().Transform(currentInputs)
 	if err != nil {
 		return nil, err
 	}

components/base/kinematicbase/fake_kinematics.go

@@ -5,6 +5,7 @@ package kinematicbase
 import (
 	"context"
 	"errors"
+	"math"
 	"sync"
 	"time"
 
@@ -20,12 +21,12 @@ import (
 
 type fakeDiffDriveKinematics struct {
 	*fake.Base
-	parentFrame                   string
-	planningFrame, executionFrame referenceframe.Frame
-	inputs                        []referenceframe.Input
-	options                       Options
-	sensorNoise                   spatialmath.Pose
-	lock                          sync.RWMutex
+	parentFrame                      string
+	planningFrame, localizationFrame referenceframe.Frame
+	inputs                           []referenceframe.Input
+	options                          Options
+	sensorNoise                      spatialmath.Pose
+	lock                             sync.RWMutex
 }
 
 // WrapWithFakeDiffDriveKinematics creates a DiffDrive KinematicBase from the fake Base so that it satisfies the ModelFramer and
@@ -57,7 +58,7 @@ func WrapWithFakeDiffDriveKinematics(
 		geometry = fk.Base.Geometry[0]
 	}
 
-	fk.executionFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, geometry)
+	fk.localizationFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, geometry)
 	if err != nil {
 		return nil, err
 	}
@@ -68,7 +69,7 @@ func WrapWithFakeDiffDriveKinematics(
 			return nil, err
 		}
 	} else {
-		fk.planningFrame = fk.executionFrame
+		fk.planningFrame = fk.localizationFrame
 	}
 
 	fk.options = options
@@ -79,6 +80,10 @@ func (fk *fakeDiffDriveKinematics) Kinematics() referenceframe.Frame {
 	return fk.planningFrame
 }
 
+func (fk *fakeDiffDriveKinematics) LocalizationFrame() referenceframe.Frame {
+	return fk.localizationFrame
+}
+
 func (fk *fakeDiffDriveKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) {
 	fk.lock.RLock()
 	defer fk.lock.RUnlock()
@@ -122,19 +127,19 @@ func (fk *fakeDiffDriveKinematics) CurrentPosition(ctx context.Context) (*refere
 
 type fakePTGKinematics struct {
 	*fake.Base
-	localizer    motion.Localizer
-	frame        referenceframe.Frame
-	options      Options
-	sensorNoise  spatialmath.Pose
-	ptgs         []tpspace.PTGSolver
-	currentInput []referenceframe.Input
-	currentIndex int
-	plan         motionplan.Plan
-	origin       *referenceframe.PoseInFrame
-	positionlock sync.RWMutex
-	inputLock    sync.RWMutex
-	logger       logging.Logger
-	sleepTime    int
+	localizer                        motion.Localizer
+	planningFrame, localizationFrame referenceframe.Frame
+	options                          Options
+	sensorNoise                      spatialmath.Pose
+	ptgs                             []tpspace.PTGSolver
+	currentInput                     []referenceframe.Input
+	currentIndex                     int
+	plan                             motionplan.Plan
+	origin                           *referenceframe.PoseInFrame
+	positionlock                     sync.RWMutex
+	inputLock                        sync.RWMutex
+	logger                           logging.Logger
+	sleepTime                        int
 }
 
 // WrapWithFakePTGKinematics creates a PTG KinematicBase from the fake Base so that it satisfies the ModelFramer and InputEnabled
@@ -180,7 +185,8 @@ func WrapWithFakePTGKinematics(
 
 	nonzeroBaseTurningRadiusMeters := (baseMillimetersPerSecond / rdkutils.DegToRad(angVelocityDegsPerSecond)) / 1000.
 
-	frame, err := tpspace.NewPTGFrameFromKinematicOptions(
+	// construct planning frame
+	planningFrame, err := tpspace.NewPTGFrameFromKinematicOptions(
 		b.Name().ShortName(),
 		logger,
 		nonzeroBaseTurningRadiusMeters,
@@ -193,30 +199,54 @@ func WrapWithFakePTGKinematics(
 		return nil, err
 	}
 
+	// construct localization frame
+	localizationFrame, err := referenceframe.NewPoseFrame(
+		b.Name().ShortName()+"LocalizationFrame",
+		[]referenceframe.Limit{
+			{Min: math.Inf(-1), Max: math.Inf(1)},
+			{Min: math.Inf(-1), Max: math.Inf(1)},
+			{Min: math.Inf(-1), Max: math.Inf(1)},
+			// we intentionally set our OX, OY, OZ values to be greater than 1 since
+			// a possible orientation which a planner produces may have the following
+			// values: OZ: 1.0000000002, Theta: t.
+			{Min: -1.5, Max: 1.5},
+			{Min: -1.5, Max: 1.5},
+			{Min: -1.5, Max: 1.5},
+			{Min: -2 * math.Pi, Max: 2 * math.Pi},
+		},
+		nil,
+	)
+	if err != nil {
+		return nil, err
+	}
+
 	if sensorNoise == nil {
 		sensorNoise = spatialmath.NewZeroPose()
 	}
 
-	ptgProv, ok := frame.(tpspace.PTGProvider)
+	ptgProv, ok := planningFrame.(tpspace.PTGProvider)
 	if !ok {
 		return nil, errors.New("unable to cast ptgk frame to a PTG Provider")
 	}
 	ptgs := ptgProv.PTGSolvers()
-	traj := motionplan.Trajectory{{frame.Name(): zeroInput}}
-	path := motionplan.Path{{frame.Name(): referenceframe.NewPoseInFrame(origin.Parent(), spatialmath.Compose(origin.Pose(), sensorNoise))}}
+	traj := motionplan.Trajectory{{planningFrame.Name(): zeroInput}}
+	path := motionplan.Path{
+		{planningFrame.Name(): referenceframe.NewPoseInFrame(origin.Parent(), spatialmath.Compose(origin.Pose(), sensorNoise))},
+	}
 	zeroPlan := motionplan.NewSimplePlan(path, traj)
 
 	fk := &fakePTGKinematics{
-		Base:         b,
-		frame:        frame,
-		origin:       origin,
-		ptgs:         ptgs,
-		currentInput: zeroInput,
-		currentIndex: 0,
-		plan:         zeroPlan,
-		sensorNoise:  sensorNoise,
-		logger:       logger,
-		sleepTime:    sleepTime,
+		Base:              b,
+		planningFrame:     planningFrame,
+		localizationFrame: localizationFrame,
+		origin:            origin,
+		ptgs:              ptgs,
+		currentInput:      zeroInput,
+		currentIndex:      0,
+		plan:              zeroPlan,
+		sensorNoise:       sensorNoise,
+		logger:            logger,
+		sleepTime:         sleepTime,
 	}
 	initLocalizer := &fakePTGKinematicsLocalizer{fk}
 	fk.localizer = motion.TwoDLocalizer(initLocalizer)
@@ -226,7 +256,11 @@ func WrapWithFakePTGKinematics(
 }
 
 func (fk *fakePTGKinematics) Kinematics() referenceframe.Frame {
-	return fk.frame
+	return fk.planningFrame
+}
+
+func (fk *fakePTGKinematics) LocalizationFrame() referenceframe.Frame {
+	return fk.localizationFrame
 }
 
 func (fk *fakePTGKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) {
@@ -241,9 +275,9 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref
 		fk.currentInput = zeroInput
 		fk.currentIndex = 0
 
-		traj := motionplan.Trajectory{{fk.frame.Name(): zeroInput}}
+		traj := motionplan.Trajectory{{fk.planningFrame.Name(): zeroInput}}
 		path := motionplan.Path{
-			{fk.frame.Name(): referenceframe.NewPoseInFrame(fk.origin.Parent(), spatialmath.Compose(fk.origin.Pose(), fk.sensorNoise))},
+			{fk.planningFrame.Name(): referenceframe.NewPoseInFrame(fk.origin.Parent(), spatialmath.Compose(fk.origin.Pose(), fk.sensorNoise))},
 		}
 		fk.plan = motionplan.NewSimplePlan(path, traj)
 		fk.inputLock.Unlock()
@@ -268,12 +302,12 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref
 
 		fk.inputLock.Lock()
 		fk.currentIndex = i
-		fk.currentInput, err = fk.frame.Interpolate(zeroInput, inputs, 0)
+		fk.currentInput, err = fk.planningFrame.Interpolate(zeroInput, inputs, 0)
 		fk.inputLock.Unlock()
 		if err != nil {
 			return err
 		}
-		finalPose, err := fk.frame.Transform(inputs)
+		finalPose, err := fk.planningFrame.Transform(inputs)
 		if err != nil {
 			return err
 		}
@@ -282,7 +316,7 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref
 		var interpolatedConfigurations [][]referenceframe.Input
 		for i := 0; i <= steps; i++ {
 			interp := float64(i) / float64(steps)
-			interpConfig, err := fk.frame.Interpolate(zeroInput, inputs, interp)
+			interpConfig, err := fk.planningFrame.Interpolate(zeroInput, inputs, interp)
 			if err != nil {
 				return err
 			}
@@ -292,7 +326,7 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref
 			if ctx.Err() != nil {
 				return ctx.Err()
 			}
-			relativePose, err := fk.frame.Transform(inter)
+			relativePose, err := fk.planningFrame.Transform(inter)
 			if err != nil {
 				return err
 			}
@@ -327,8 +361,8 @@ func (fk *fakePTGKinematics) ExecutionState(ctx context.Context) (motionplan.Exe
 	return motionplan.NewExecutionState(
 		fk.plan,
 		fk.currentIndex,
-		map[string][]referenceframe.Input{fk.frame.Name(): fk.currentInput},
-		map[string]*referenceframe.PoseInFrame{fk.frame.Name(): pos},
+		map[string][]referenceframe.Input{fk.Kinematics().Name(): fk.currentInput},
+		map[string]*referenceframe.PoseInFrame{fk.LocalizationFrame().Name(): pos},
 	)
 }
 

components/base/kinematicbase/kinematics.go

@@ -24,6 +24,7 @@ type KinematicBase interface {
 	referenceframe.InputEnabled
 
 	Kinematics() referenceframe.Frame
+	LocalizationFrame() referenceframe.Frame
 
 	// ErrorState takes a complete motionplan, as well as the index of the currently-executing set of inputs, and computes the pose
 	// difference between where the robot in fact is, and where it ought to be, i.e. PoseBetween(expected, actual)