course correction bug fix + logging erogonmics (#3810)

viamrobotics · Apr 17, 2024 · 5475b49 · 5475b49
1 parent d96212a
commit 5475b49
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 5 deletions.
diff --git a/components/base/kinematicbase/execution.go b/components/base/kinematicbase/execution.go
@@ -49,11 +49,11 @@ type arcStep struct {
 }
 
 func (step *arcStep) String() string {
-	return fmt.Sprintf("Step: lin velocity: %f, ang velocity: %f, duration: %f s, arcSegment %+v, arc start pose %s",
+	return fmt.Sprintf("Step: lin velocity: %f,\n\t ang velocity: %f,\n\t duration: %f s,\n\t arcSegment %s,\n\t arc start pose %v",
 		step.linVelMMps,
 		step.angVelDegps,
 		step.durationSeconds,
-		step.arcSegment,
+		step.arcSegment.String(),
 		spatialmath.PoseToProtobuf(step.arcSegment.StartPosition),
 	)
 }
@@ -153,7 +153,7 @@ func (ptgk *ptgBaseKinematics) GoToInputs(ctx context.Context, inputSteps ...[]r
 			currentInputs := []referenceframe.Input{
 				step.arcSegment.StartConfiguration[0],
 				step.arcSegment.StartConfiguration[1],
-				{step.arcSegment.StartConfiguration[2].Value + math.Abs(distIncVel)*timeElapsedSeconds},
+				{Value: step.arcSegment.StartConfiguration[2].Value + math.Abs(distIncVel)*timeElapsedSeconds},
 			}
 			ptgk.inputLock.Lock()
 			ptgk.currentInputs = currentInputs
@@ -353,6 +353,7 @@ func (ptgk *ptgBaseKinematics) courseCorrect(
 			ptgk.logger.Debug("successful course correction", solution.Solution)
 
 			correctiveArcSteps := []arcStep{}
+			actualPoseTracked := actualPose.Pose()
 			for i := 0; i < len(solution.Solution); i += 2 {
 				// We've got a course correction solution. Swap out the relevant arcsteps.
 				correctiveTraj, err := ptgk.ptgs[ptgk.courseCorrectionIdx].Trajectory(
@@ -363,14 +364,21 @@ func (ptgk *ptgBaseKinematics) courseCorrect(
 				if err != nil {
 					return nil, err
 				}
+
 				newArcSteps, err := ptgk.trajectoryToArcSteps(
 					correctiveTraj,
-					actualPose.Pose(),
+					actualPoseTracked,
 					[]referenceframe.Input{{float64(ptgk.courseCorrectionIdx)}, solution.Solution[i], solution.Solution[i+1]},
 				)
 				if err != nil {
 					return nil, err
 				}
+				for _, newArcStep := range newArcSteps {
+					actualPoseTracked = spatialmath.Compose(
+						actualPoseTracked,
+						spatialmath.PoseBetween(newArcStep.arcSegment.StartPosition, newArcStep.arcSegment.EndPosition),
+					)
+				}
 				correctiveArcSteps = append(correctiveArcSteps, newArcSteps...)
 			}
 
@@ -479,7 +487,6 @@ func (ptgk *ptgBaseKinematics) makeCourseCorrectionGoals(
 	for i := currStep; i < len(steps); i++ {
 		for len(steps[i].subTraj)-startingTrajPt > stepsRemainingThisGoal {
 			goalTrajPtIdx := startingTrajPt + stepsRemainingThisGoal
-
 			goalPose := spatialmath.PoseBetween(
 				currPose,
 				spatialmath.Compose(steps[i].arcSegment.StartPosition, steps[i].subTraj[goalTrajPtIdx].Pose),

diff --git a/motionplan/ik/metrics.go b/motionplan/ik/metrics.go
@@ -1,6 +1,7 @@
 package ik
 
 import (
+	"fmt"
 	"math"
 
 	"go.viam.com/rdk/referenceframe"
@@ -21,6 +22,16 @@ type Segment struct {
 	Frame              referenceframe.Frame
 }
 
+func (s *Segment) String() string {
+	return fmt.Sprintf("Segment: StartPosition: %v,\n\t EndPosition: %v,\n\t StartConfiguration:%v,\n\t EndConfiguration:%v,\n\t Frame: %v",
+		spatial.PoseToProtobuf(s.StartPosition),
+		spatial.PoseToProtobuf(s.EndPosition),
+		s.StartConfiguration,
+		s.EndConfiguration,
+		s.Frame,
+	)
+}
+
 // State contains all the information a constraint needs to determine validity for a movement.
 // It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to.
 // Pose fields may be empty, and may be filled in by a constraint that needs them.