RSDK-13688: Track PerGoal meta information in TestBadSpray1. Output it on test failure.

motionplan/armplanning/api.go

@@ -192,6 +192,9 @@ type SolutionNodeInfo struct {
 // PerGoalMeta holds diagnostic data for a single invocation of initRRTSolutions.
 // Only populated when PlannerOptions.CollectSolutionDiagnostics is true.
 type PerGoalMeta struct {
+	StartConfiguration *referenceframe.LinearInputs
+	GoalPoses          referenceframe.FrameSystemPoses
+	ReasonableCost     float64
 	// SolutionNodes contains info about each IK solution node scored and path-checked.
 	SolutionNodes []SolutionNodeInfo
 	// ConstraintFailuresByType maps constraint error strings to the number of IK candidate
@@ -201,10 +204,19 @@ type PerGoalMeta struct {
 
 // PlanMeta is meta data about plan generation.
 type PlanMeta struct {
-	Duration       time.Duration
-	Partial        bool
-	PartialError   error
+	Duration     time.Duration
+	Partial      bool
+	PartialError error
+	// GoalsProcessed is how many user-defined goals were solved for.
 	GoalsProcessed int
+	// SubgoalsPerGoal will have size of `GoalsProcessed`. If there are no linear/orientation
+	// constraints, we do not create any additional subgoals/waypoints. SubgoalsPerGoal in that case
+	// will be set to 1 for each goal index. Otherwise it will be sent to the number of internal
+	// waypoints created + 1 (for the final user goal).
+	SubgoalsPerGoal []int
+	// SubgoalsProcessed may be non-zero when a plan request has linear/orientation
+	// constraints. Satisfying those constraints internally creates additional goals.
+	SubgoalsProcessed int
 
 	// CollectSolutionDiagnostics is copied from PlannerOptions and gates whether PerGoal is
 	// populated.
@@ -215,6 +227,40 @@ type PlanMeta struct {
 	PerGoal []PerGoalMeta
 }
 
+// OutputToLogger dumps all PerGoal diagnostic data to the logger in a human-readable format.
+// Only useful when CollectSolutionDiagnostics was set on the PlannerOptions.
+func (meta *PlanMeta) OutputToLogger(logger logging.Logger) {
+	logger.Infof("PlanMeta: duration=%v partial=%v goalsProcessed=%d subgoalsProcessed=%d subgoalsPerGoal=%v",
+		meta.Duration, meta.Partial, meta.GoalsProcessed, meta.SubgoalsProcessed, meta.SubgoalsPerGoal)
+	if meta.PartialError != nil {
+		logger.Infof("  partialError: %v", meta.PartialError)
+	}
+
+	if !meta.CollectSolutionDiagnostics {
+		logger.Infof("  (CollectSolutionDiagnostics was false — no PerGoal data)")
+		return
+	}
+
+	logger.Infof("  perGoal entries: %d", len(meta.PerGoal))
+	for idx, pg := range meta.PerGoal {
+		logger.Infof("  [goal %d] startConfig=%v goal=%v reasonableCost=%.4f solutionNodes=%d constraintFailures=%d",
+			idx, pg.StartConfiguration, pg.GoalPoses, pg.ReasonableCost, len(pg.SolutionNodes), len(pg.ConstraintFailuresByType))
+		for failType, count := range pg.ConstraintFailuresByType {
+			logger.Infof("    constraintFailure %q: %d", failType, count)
+		}
+
+		for nodeIdx, node := range pg.SolutionNodes {
+			if node.CheckPathError != nil {
+				logger.Infof("    [node %d] score=%.4f checkPathError=%v lastGoodInputs=%v inputs=%v",
+					nodeIdx, node.Score, node.CheckPathError, node.LastGoodInputs, node.Inputs)
+			} else {
+				logger.Infof("    [node %d] score=%.4f checkPath=ok inputs=%v",
+					nodeIdx, node.Score, node.Inputs)
+			}
+		}
+	}
+}
+
 // PlanMotion plans a motion from a provided plan request.
 func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanRequest) (motionplan.Plan, *PlanMeta, error) {
 	logger := parentLogger.Sublogger("mp")
@@ -258,6 +304,7 @@ func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanR
 	}
 
 	trajAsInps, goalsProcessed, err := sfPlanner.planMultiWaypoint(ctx)
+	meta.GoalsProcessed = goalsProcessed
 	if err != nil {
 		if request.PlannerOptions.ReturnPartialPlan {
 			meta.Partial = true
@@ -268,8 +315,6 @@ func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanR
 		}
 	}
 
-	meta.GoalsProcessed = goalsProcessed
-
 	t, err := motionplan.NewSimplePlanFromTrajectory(trajAsInps, request.FrameSystem)
 	if err != nil {
 		return nil, meta, err

motionplan/armplanning/cBiRRT.go

@@ -33,15 +33,15 @@ var debugConstrainNear = false
 // It uses the Constrained Bidirctional Rapidly-expanding Random Tree algorithm, Berenson et al 2009
 // https://ieeexplore.ieee.org/document/5152399/
 type cBiRRTMotionPlanner struct {
-	pc     *planContext
-	psc    *planSegmentContext
+	pc     *PlanContext
+	psc    *PlanSegmentContext
 	logger logging.Logger
 
 	fastGradDescent *ik.NloptIK
 }
 
 // newCBiRRTMotionPlannerWithSeed creates a cBiRRTMotionPlanner object with a user specified random seed.
-func newCBiRRTMotionPlanner(ctx context.Context, pc *planContext, psc *planSegmentContext, logger logging.Logger,
+func newCBiRRTMotionPlanner(ctx context.Context, pc *PlanContext, psc *PlanSegmentContext, logger logging.Logger,
 ) (*cBiRRTMotionPlanner, error) {
 	_, span := trace.StartSpan(ctx, "newCBiRRTMotionPlanner")
 	defer span.End()
@@ -145,7 +145,7 @@ func (mp *cBiRRTMotionPlanner) rrtRunner(
 
 		map1reached, map2reached := tryExtend(target)
 
-		reachedDelta := mp.pc.configurationDistanceFunc(
+		reachedDelta := mp.pc.ConfigurationDistanceFunc(
 			&motionplan.SegmentFS{
 				StartConfiguration: map1reached.inputs,
 				EndConfiguration:   map2reached.inputs,
@@ -161,7 +161,7 @@ func (mp *cBiRRTMotionPlanner) rrtRunner(
 			target = newConfigurationNode(targetConf)
 			map1reached, map2reached = tryExtend(target)
 
-			reachedDelta = mp.pc.configurationDistanceFunc(&motionplan.SegmentFS{
+			reachedDelta = mp.pc.ConfigurationDistanceFunc(&motionplan.SegmentFS{
 				StartConfiguration: map1reached.inputs,
 				EndConfiguration:   map2reached.inputs,
 			})
@@ -209,7 +209,7 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend(
 	// 4) further iterations change our best node by close-to-zero amounts
 	// 5) we have iterated more than maxExtendIter times
 	for i := 0; i < maxExtendIter; i++ {
-		configDistMetric := mp.pc.configurationDistanceFunc
+		configDistMetric := mp.pc.ConfigurationDistanceFunc
 		dist := configDistMetric(
 			&motionplan.SegmentFS{StartConfiguration: near.inputs, EndConfiguration: target.inputs})
 		oldDist := configDistMetric(
@@ -232,7 +232,7 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend(
 			return oldNear
 		}
 
-		nearDist := mp.pc.configurationDistanceFunc(
+		nearDist := mp.pc.ConfigurationDistanceFunc(
 			&motionplan.SegmentFS{StartConfiguration: oldNear.inputs, EndConfiguration: newNear})
 
 		if nearDist < math.Pow(mp.pc.planOpts.InputIdentDist, 3) {
@@ -281,7 +281,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
 	}
 
 	// Check if the arc of "seedInputs" to "target" is valid
-	_, err := mp.psc.checker.CheckStateConstraintsAcrossSegmentFS(ctx, newArc, mp.pc.planOpts.Resolution, true)
+	_, err := mp.psc.Checker.CheckStateConstraintsAcrossSegmentFS(ctx, newArc, mp.pc.planOpts.Resolution, true)
 	if debugConstrainNear {
 		mp.logger.Infof("\t err %v", err)
 	}
@@ -320,7 +320,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
 		linearSeed := target.GetLinearizedInputs()
 		var totalAttempts atomic.Int32
 		solutions, _, err := ik.DoSolve(ctx, mp.fastGradDescent, &totalAttempts,
-			mp.psc.pc.linearizeFSmetric(myFunc),
+			mp.psc.pc.LinearizeFSMetric(myFunc),
 			[][]float64{linearSeed}, [][]referenceframe.Limit{ik.ComputeAdjustLimits(linearSeed, mp.pc.lis.GetLimits(), .05)})
 		if err != nil {
 			mp.logger.Debugf("constrainNear fail (DoSolve): %v", err)
@@ -342,7 +342,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
 		}
 	}
 
-	failpos, err := mp.psc.checker.CheckStateConstraintsAcrossSegmentFS(
+	failpos, err := mp.psc.Checker.CheckStateConstraintsAcrossSegmentFS(
 		ctx,
 		&motionplan.SegmentFS{
 			StartConfiguration: seedInputs,
@@ -364,7 +364,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
 		return nil
 	}
 
-	dist := mp.pc.configurationDistanceFunc(&motionplan.SegmentFS{
+	dist := mp.pc.ConfigurationDistanceFunc(&motionplan.SegmentFS{
 		StartConfiguration: seedInputs,
 		EndConfiguration:   failpos.EndConfiguration,
 	})

motionplan/armplanning/cBiRRT_test.go

@@ -40,10 +40,10 @@ func TestSimpleLinearMotion(t *testing.T) {
 		Constraints:    &motionplan.Constraints{},
 	}
 
-	pc, err := newPlanContext(ctx, logger, request, &PlanMeta{})
+	pc, err := NewPlanContext(ctx, logger, request, &PlanMeta{})
 	test.That(t, err, test.ShouldBeNil)
 
-	psc, err := newPlanSegmentContext(ctx, pc, referenceframe.FrameSystemInputs{m.Name(): home7}.ToLinearInputs(), goal)
+	psc, err := NewPlanSegmentContext(ctx, pc, referenceframe.FrameSystemInputs{m.Name(): home7}.ToLinearInputs(), goal)
 	test.That(t, err, test.ShouldBeNil)
 
 	mp, err := newCBiRRTMotionPlanner(ctx, pc, psc, logger.Sublogger("cbirrt"))
@@ -83,7 +83,7 @@ func TestSimpleLinearMotion(t *testing.T) {
 	// extend goalMap towards the point in seedMap
 	goalReached := mp.constrainedExtend(ctx, 1, goalMap, near2, seedReached)
 
-	dist := pc.configurationDistanceFunc(
+	dist := pc.ConfigurationDistanceFunc(
 		&motionplan.SegmentFS{StartConfiguration: seedReached.inputs, EndConfiguration: goalReached.inputs},
 	)
 	test.That(t, dist, test.ShouldBeLessThan, pc.planOpts.InputIdentDist)

motionplan/armplanning/constraint_test.go

@@ -40,10 +40,10 @@ func TestIKTolerances(t *testing.T) {
 		Constraints:    &motionplan.Constraints{},
 	}
 
-	pc, err := newPlanContext(ctx, logger, request, &PlanMeta{})
+	pc, err := NewPlanContext(ctx, logger, request, &PlanMeta{})
 	test.That(t, err, test.ShouldBeNil)
 
-	psc, err := newPlanSegmentContext(ctx, pc, seed, goal)
+	psc, err := NewPlanSegmentContext(ctx, pc, seed, goal)
 	test.That(t, err, test.ShouldBeNil)
 
 	mp, err := newCBiRRTMotionPlanner(ctx, pc, psc, logger.Sublogger("cbirrt"))
@@ -66,10 +66,10 @@ func TestIKTolerances(t *testing.T) {
 		Constraints:    &motionplan.Constraints{},
 	}
 
-	pc2, err := newPlanContext(ctx, logger, request2, &PlanMeta{})
+	pc2, err := NewPlanContext(ctx, logger, request2, &PlanMeta{})
 	test.That(t, err, test.ShouldBeNil)
 
-	psc2, err := newPlanSegmentContext(ctx, pc2, seed, goal)
+	psc2, err := NewPlanSegmentContext(ctx, pc2, seed, goal)
 	test.That(t, err, test.ShouldBeNil)
 
 	mp2, err := newCBiRRTMotionPlanner(ctx, pc2, psc2, logger.Sublogger("cbirrt"))

motionplan/armplanning/context.go

@@ -16,13 +16,13 @@
 	"go.viam.com/rdk/referenceframe"
 )
 
-type planContext struct {
+type PlanContext struct {
 	fs  *referenceframe.FrameSystem
 	lis *referenceframe.LinearInputsSchema
 
 	movableFrames []string
 
-	configurationDistanceFunc motionplan.SegmentFSMetric
+	ConfigurationDistanceFunc motionplan.SegmentFSMetric
 	planOpts                  *PlannerOptions
 	request                   *PlanRequest
 
@@ -39,13 +39,13 @@
 	collisionCache *motionplan.CollisionCache
 }
 
-func newPlanContext(ctx context.Context, logger logging.Logger, request *PlanRequest, meta *PlanMeta) (*planContext, error) {
-	_, span := trace.StartSpan(ctx, "newPlanContext")
+func NewPlanContext(ctx context.Context, logger logging.Logger, request *PlanRequest, meta *PlanMeta) (*PlanContext, error) {
+	_, span := trace.StartSpan(ctx, "NewPlanContext")
 	defer span.End()
 	meta.CollectSolutionDiagnostics = request.PlannerOptions.CollectSolutionDiagnostics
-	pc := &planContext{
+	pc := &PlanContext{
 		fs:                        request.FrameSystem,
-		configurationDistanceFunc: motionplan.GetConfigurationDistanceFunc(request.PlannerOptions.ConfigurationDistanceMetric),
+		ConfigurationDistanceFunc: motionplan.GetConfigurationDistanceFunc(request.PlannerOptions.ConfigurationDistanceMetric),
 		planOpts:                  request.PlannerOptions,
 		request:                   request,
 		randseed:                  rand.New(rand.NewSource(int64(request.PlannerOptions.RandomSeed))), //nolint:gosec
@@ -70,7 +70,11 @@
 	return pc, nil
 }
 
-func (pc *planContext) linearizeFSmetric(metric motionplan.StateFSMetric) ik.CostFunc {
+func (pc *PlanContext) GetLinearInputsSchema() *referenceframe.LinearInputsSchema {
+	return pc.lis
+}
+
+func (pc *PlanContext) LinearizeFSMetric(metric motionplan.StateFSMetric) ik.CostFunc {
 	return func(ctx context.Context, linearizedInputs []float64) float64 {
 		conf, err := pc.lis.FloatsToInputs(linearizedInputs)
 		if err != nil {
@@ -84,8 +88,8 @@
 	}
 }
 
-type planSegmentContext struct {
-	pc *planContext
+type PlanSegmentContext struct {
+	pc *PlanContext
 
 	start    *referenceframe.LinearInputs
 	origGoal referenceframe.FrameSystemPoses // goals are defined in frames willy nilly
@@ -94,15 +98,15 @@
 	startPoses referenceframe.FrameSystemPoses
 
 	motionChains *motionChains
-	checker      *motionplan.ConstraintChecker
+	Checker      *motionplan.ConstraintChecker
 }
 
-func newPlanSegmentContext(ctx context.Context, pc *planContext, start *referenceframe.LinearInputs,
+func NewPlanSegmentContext(ctx context.Context, pc *PlanContext, start *referenceframe.LinearInputs,
 	goal referenceframe.FrameSystemPoses,
-) (*planSegmentContext, error) {
-	_, span := trace.StartSpan(ctx, "newPlanSegmentContext")
+) (*PlanSegmentContext, error) {
+	_, span := trace.StartSpan(ctx, "NewPlanSegmentContext")
 	defer span.End()
-	psc := &planSegmentContext{
+	psc := &PlanSegmentContext{
 		pc:       pc,
 		start:    start,
 		origGoal: goal,
@@ -139,7 +143,7 @@
 
 	movingRobotGeometries, staticRobotGeometries := psc.motionChains.geometries(pc.fs, frameSystemGeometries)
 
-	psc.checker, err = motionplan.NewConstraintChecker(
+	psc.Checker, err = motionplan.NewConstraintChecker(
 		pc.planOpts.CollisionBufferMM,
 		pc.request.Constraints,
 		psc.startPoses,
@@ -158,11 +162,11 @@
 	return psc, nil
 }
 
-// checkPath returns an error if the interpolation between `start` and `end` violate a constraint
+// CheckPath returns an error if the interpolation between `start` and `end` violate a constraint
 // (e.g: we calculcate there will be a collision). If there is an error and `outPath` is non-nil,
 // `outPath` will be populated with more detailed information.
-func (psc *planSegmentContext) checkPath(
-	ctx context.Context, start, end *referenceframe.LinearInputs, checkFinal bool, outPath *pathFeedback,
+func (psc *PlanSegmentContext) CheckPath(
+	ctx context.Context, start, end *referenceframe.LinearInputs, checkFinal bool, outPath *PathFeedback,
 ) error {
 	ctx, span := trace.StartSpan(ctx, "checkPath")
 	defer span.End()
@@ -181,7 +185,7 @@
 		}
 	}
 
-	validSegment, err := psc.checker.CheckStateConstraintsAcrossSegmentFS(
+	validSegment, err := psc.Checker.CheckStateConstraintsAcrossSegmentFS(
 		ctx,
 		&motionplan.SegmentFS{
 			StartConfiguration: start,
@@ -196,7 +200,7 @@
 	}
 
 	if err != nil && outPath != nil {
-		*outPath = pathFeedback{
+		*outPath = PathFeedback{
 			IsObstacleCollision: strings.Contains(err.Error(), motionplan.ObstacleConstraintDescription) ||
 				strings.Contains(err.Error(), motionplan.RobotCollisionConstraintDescription),
 			LastGoodInputs: validSegment.EndConfiguration,
@@ -242,7 +246,7 @@
 	return alteredGoals, nil
 }
 
-func (pc *planContext) isFatalCollision(err error) bool {
+func (pc *PlanContext) isFatalCollision(err error) bool {
 	s := err.Error()
 	if strings.Contains(s, "obstacle constraint: violation") {
 		hasMovingFrame := false

motionplan/armplanning/functional_test.go

@@ -298,10 +298,10 @@ func testPlanner(t *testing.T, ctx context.Context, config planConfigConstructor
 		Constraints:    cfg.Constraints,
 	}
 
-	pc, err := newPlanContext(ctx, logger, request, &PlanMeta{})
+	pc, err := NewPlanContext(ctx, logger, request, &PlanMeta{})
 	test.That(t, err, test.ShouldBeNil)
 
-	psc, err := newPlanSegmentContext(ctx, pc, cfg.Start.LinearConfiguration(), cfg.Goal.poses)
+	psc, err := NewPlanSegmentContext(ctx, pc, cfg.Start.LinearConfiguration(), cfg.Goal.poses)
 	test.That(t, err, test.ShouldBeNil)
 
 	mp, err := newCBiRRTMotionPlanner(ctx, pc, psc, logger.Sublogger("cbirrt"))