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

components/base/kinematicbase/differentialDrive.go

@@ -111,6 +111,10 @@ func (ddk *differentialDriveKinematics) Kinematics() referenceframe.Frame {
 	return ddk.planningFrame
 }
 
+func (ddk *differentialDriveKinematics) ExecutionFrame() referenceframe.Frame {
+	return ddk.executionFrame
+}
+
 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.

components/base/kinematicbase/execution.go

@@ -322,7 +322,7 @@ func (ptgk *ptgBaseKinematics) courseCorrect(
 		arcSteps[arcIdx].arcSegment.StartConfiguration[startDistanceAlongTrajectoryIndex],
 		currentInputs[startDistanceAlongTrajectoryIndex],
 	}
-	trajPose, err := ptgk.frame.Transform(execInputs)
+	trajPose, err := ptgk.planningFrame.Transform(execInputs)
 	if err != nil {
 		return nil, err
 	}

components/base/kinematicbase/fake_kinematics.go

@@ -79,6 +79,10 @@ func (fk *fakeDiffDriveKinematics) Kinematics() referenceframe.Frame {
 	return fk.planningFrame
 }
 
+func (fk *fakeDiffDriveKinematics) ExecutionFrame() referenceframe.Frame {
+	return fk.executionFrame
+}
+
 func (fk *fakeDiffDriveKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) {
 	fk.lock.RLock()
 	defer fk.lock.RUnlock()
@@ -118,17 +122,17 @@ 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
-	origin       *referenceframe.PoseInFrame
-	positionlock sync.RWMutex
-	inputLock    sync.RWMutex
-	logger       logging.Logger
-	sleepTime    int
+	localizer                     motion.Localizer
+	planningFrame, executionFrame referenceframe.Frame
+	options                       Options
+	sensorNoise                   spatialmath.Pose
+	ptgs                          []tpspace.PTGSolver
+	currentInput                  []referenceframe.Input
+	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
@@ -174,7 +178,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,
@@ -187,25 +192,36 @@ func WrapWithFakePTGKinematics(
 		return nil, err
 	}
 
+	// construct execution frame
+	executionFrame, err := referenceframe.New2DMobileModelFrame(
+		b.Name().ShortName()+"ExecutionFrame",
+		planningFrame.DoF(),
+		geometries[0],
+	)
+	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()
 
 	fk := &fakePTGKinematics{
-		Base:         b,
-		frame:        frame,
-		origin:       origin,
-		ptgs:         ptgs,
-		currentInput: zeroInput,
-		sensorNoise:  sensorNoise,
-		logger:       logger,
-		sleepTime:    sleepTime,
+		Base:           b,
+		planningFrame:  planningFrame,
+		executionFrame: executionFrame,
+		origin:         origin,
+		ptgs:           ptgs,
+		currentInput:   zeroInput,
+		sensorNoise:    sensorNoise,
+		logger:         logger,
+		sleepTime:      sleepTime,
 	}
 	initLocalizer := &fakePTGKinematicsLocalizer{fk}
 	fk.localizer = motion.TwoDLocalizer(initLocalizer)
@@ -215,7 +231,11 @@ func WrapWithFakePTGKinematics(
 }
 
 func (fk *fakePTGKinematics) Kinematics() referenceframe.Frame {
-	return fk.frame
+	return fk.planningFrame
+}
+
+func (fk *fakePTGKinematics) ExecutionFrame() referenceframe.Frame {
+	return fk.executionFrame
 }
 
 func (fk *fakePTGKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) {
@@ -240,7 +260,7 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref
 		fk.currentInput = []referenceframe.Input{inputs[0], inputs[1], {Value: 0}}
 		fk.inputLock.Unlock()
 
-		finalPose, err := fk.frame.Transform(inputs)
+		finalPose, err := fk.planningFrame.Transform(inputs)
 		if err != nil {
 			return err
 		}
@@ -250,7 +270,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(startCfg, inputs, interp)
+			interpConfig, err := fk.planningFrame.Interpolate(startCfg, inputs, interp)
 			if err != nil {
 				return err
 			}
@@ -260,7 +280,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
 			}

components/base/kinematicbase/kinematics.go

@@ -23,6 +23,8 @@ type KinematicBase interface {
 	referenceframe.InputEnabled
 
 	Kinematics() referenceframe.Frame
+	ExecutionFrame() 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.
 	ErrorState(context.Context) (spatialmath.Pose, error)

components/base/kinematicbase/ptgKinematics.go

@@ -31,7 +31,7 @@ type ptgBaseKinematics struct {
 	base.Base
 	motion.Localizer
 	logger                         logging.Logger
-	frame                          referenceframe.Frame
+	planningFrame, executionFrame  referenceframe.Frame
 	ptgs                           []tpspace.PTGSolver
 	courseCorrectionIdx            int
 	linVelocityMMPerSecond         float64
@@ -100,7 +100,7 @@ func wrapWithPTGKinematics(
 	}
 
 	nonzeroBaseTurningRadiusMeters := (linVelocityMMPerSecond / rdkutils.DegToRad(angVelocityDegsPerSecond)) / 1000.
-	frame, err := tpspace.NewPTGFrameFromKinematicOptions(
+	planningFrame, err := tpspace.NewPTGFrameFromKinematicOptions(
 		b.Name().ShortName(),
 		logger,
 		nonzeroBaseTurningRadiusMeters,
@@ -112,14 +112,14 @@ func wrapWithPTGKinematics(
 	if err != nil {
 		return nil, err
 	}
-	ptgProv, err := rdkutils.AssertType[tpspace.PTGProvider](frame)
+	ptgProv, err := rdkutils.AssertType[tpspace.PTGProvider](planningFrame)
 	if err != nil {
 		return nil, err
 	}
 	ptgs := ptgProv.PTGSolvers()
 	origin := spatialmath.NewZeroPose()
 
-	ptgCourseCorrection, err := rdkutils.AssertType[tpspace.PTGCourseCorrection](frame)
+	ptgCourseCorrection, err := rdkutils.AssertType[tpspace.PTGCourseCorrection](planningFrame)
 	if err != nil {
 		return nil, err
 	}
@@ -133,11 +133,22 @@ func wrapWithPTGKinematics(
 		origin = originPIF.Pose()
 	}
 
+	// construct executionFrame
+	executionFrame, err := referenceframe.New2DMobileModelFrame(
+		b.Name().ShortName()+"ExecutionFrame",
+		planningFrame.DoF(),
+		geometries[0],
+	)
+	if err != nil {
+		return nil, err
+	}
+
 	return &ptgBaseKinematics{
 		Base:                           b,
 		Localizer:                      localizer,
 		logger:                         logger,
-		frame:                          frame,
+		planningFrame:                  planningFrame,
+		executionFrame:                 executionFrame,
 		ptgs:                           ptgs,
 		courseCorrectionIdx:            courseCorrectionIdx,
 		linVelocityMMPerSecond:         linVelocityMMPerSecond,
@@ -150,7 +161,11 @@ func wrapWithPTGKinematics(
 }
 
 func (ptgk *ptgBaseKinematics) Kinematics() referenceframe.Frame {
-	return ptgk.frame
+	return ptgk.planningFrame
+}
+
+func (ptgk *ptgBaseKinematics) ExecutionFrame() referenceframe.Frame {
+	return ptgk.executionFrame
 }
 
 // For a ptgBaseKinematics, `CurrentInputs` returns inputs which reflect what the base is currently doing.
@@ -193,7 +208,7 @@ func (ptgk *ptgBaseKinematics) ErrorState(ctx context.Context) (spatialmath.Pose
 		currentInputs[startDistanceAlongTrajectoryIndex],
 	}
 
-	currPoseInArc, err := ptgk.frame.Transform(executedInputs)
+	currPoseInArc, err := ptgk.planningFrame.Transform(executedInputs)
 	if err != nil {
 		return nil, err
 	}

motionplan/motionPlanner.go

@@ -17,6 +17,7 @@ import (
 
 	"go.viam.com/rdk/logging"
 	"go.viam.com/rdk/motionplan/ik"
+	"go.viam.com/rdk/referenceframe"
 	frame "go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/spatialmath"
 )
@@ -477,24 +478,9 @@ func CheckPlan(
 		return err
 	}
 
-	var startPose spatialmath.Pose
-	if relative {
-		// A frame's transformation based on a relative input will position it relative to the
-		// frame's origin, giving us a relative pose. To put it with respect to the world
-		// we compose the relative pose with the most recent former pose we have already reached.
-		if wayPointIdx > 0 {
-			startPose = poses[wayPointIdx-1]
-		} else {
-			// If waypointIdx is 0, we have not begun the plan yet and thus the start pose will be the first pose.
-			startPose = poses[wayPointIdx]
-		}
-	} else {
-		startPose = currentPose
-	}
-
 	// setup the planOpts
 	if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest(
-		startPose,
+		currentPose,
 		poses[len(poses)-1],
 		currentInputs,
 		worldState,
@@ -516,7 +502,7 @@ func CheckPlan(
 
 		// pre-pend to segments so we can connect to the input we have not finished actuating yet
 		segments = append(segments, &ik.Segment{
-			StartPosition:      startPose,
+			StartPosition:      currentPose,
 			EndPosition:        poses[wayPointIdx],
 			StartConfiguration: checkFrameCurrentInputs,
 			EndConfiguration:   checkFrameCurrentInputs,
@@ -529,6 +515,7 @@ func CheckPlan(
 		currPose, nextPose spatialmath.Pose,
 		currInput, nextInput map[string][]frame.Input,
 	) (*ik.Segment, error) {
+		// there will be an issue here since currInputs is not of the correct length
 		currInputSlice, err := sf.mapToSlice(currInput)
 		if err != nil {
 			return nil, err
@@ -553,7 +540,18 @@ func CheckPlan(
 
 	// iterate through remaining plan and append remaining segments to check
 	for i := wayPointIdx; i < len(offsetPlan.Path())-1; i++ {
-		segment, err := createSegment(poses[i], poses[i+1], offsetPlan.Trajectory()[i], offsetPlan.Trajectory()[i+1])
+		// do we want to add a relative special case here or in the anon function
+		currInput := offsetPlan.Trajectory()[i]
+		nextInput := offsetPlan.Trajectory()[i+1]
+		if relative {
+			currInput[checkFrame.Name()+"ExecutionFrame"] = referenceframe.FloatsToInputs(
+				[]float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta},
+			)
+			nextInput[checkFrame.Name()+"ExecutionFrame"] = referenceframe.FloatsToInputs(
+				[]float64{poses[i+1].Point().X, poses[i+1].Point().Y, poses[i+1].Orientation().OrientationVectorRadians().Theta},
+			)
+		}
+		segment, err := createSegment(poses[i], poses[i+1], currInput, nextInput)
 		if err != nil {
 			return err
 		}
@@ -581,16 +579,20 @@ func CheckPlan(
 				return nil
 			}
 
-			// If we are working with a PTG plan the returned value for poseInPath will only
-			// tell us how far along the arc we have traveled. Since this is only the relative position,
-			//  i.e. relative to where the robot started executing the arc,
-			// we must compose poseInPath with segment.StartPosition to get the absolute position.
+			// Add new comment here
 			interpolatedState := &ik.State{Frame: sf}
 			if relative {
-				interpolatedState.Position = spatialmath.Compose(segment.StartPosition, poseInPath)
+				// I AM PRETTY SURE THAT ORDER MATTERS HERE IN TERMS OF STUFF?
+				interpolatedState.Configuration = []frame.Input{
+					{Value: segment.StartPosition.Point().X},
+					{Value: segment.StartPosition.Point().Y},
+					{Value: segment.StartPosition.Orientation().OrientationVectorDegrees().Theta},
+					interpConfig[0], interpConfig[1], interpConfig[2],
+				}
 			} else {
 				interpolatedState.Configuration = interpConfig
 			}
+			fmt.Println("interpolatedState: ", interpolatedState)
 
 			// Checks for collision along the interpolated route and returns a the first interpolated pose where a collision is detected.
 			if isValid, err := sfPlanner.planOpts.CheckStateConstraints(interpolatedState); !isValid {

motionplan/plan.go

@@ -172,7 +172,7 @@ func (path Path) String() string {
 	for _, step := range path {
 		str += "\n"
 		for frame, pose := range step {
-			str += fmt.Sprintf("%s: %v %v\t", frame, pose.Pose().Point(), pose.Pose().Orientation().OrientationVectorDegrees())
+			str += fmt.Sprintf("%s: %v %v\t", frame, pose.Pose().Point(), *pose.Pose().Orientation().OrientationVectorDegrees())
 		}
 	}
 	return str

motionplan/planManager.go

@@ -504,18 +504,6 @@ func (pm *planManager) plannerSetupFromMoveRequest(
 		return nil, err // no geometries defined for frame
 	}
 	movingRobotGeometries := movingGeometriesInFrame.Geometries() // solver frame returns geoms in frame World
-	if pm.useTPspace {
-		// If we are starting a ptg plan at a different place than the origin, then that translation must be represented in the geometries,
-		// all of which need to be in the "correct" position when transformed to the world frame.
-		// At this point in the plan manager, a ptg plan is moving to a goal in the world frame, and the start pose is the base's location
-		// relative to world. Since nothing providing a geometry knows anything about where the base is relative to world, any geometries
-		// need to be transformed by the start position to place them correctly in world.
-		startGeoms := make([]spatialmath.Geometry, 0, len(movingRobotGeometries))
-		for _, geometry := range movingRobotGeometries {
-			startGeoms = append(startGeoms, geometry.Transform(from))
-		}
-		movingRobotGeometries = startGeoms
-	}
 
 	// find all geometries that are not moving but are in the frame system
 	staticRobotGeometries := make([]spatialmath.Geometry, 0)