Feed live IK solutions to cbirrt. Add request file that does not prod…

…uce enough to start.

go.mod

@@ -48,6 +48,7 @@ require (
 	github.com/jedib0t/go-pretty/v6 v6.4.6
 	github.com/jhump/protoreflect v1.15.6
 	github.com/kellydunn/golang-geo v0.7.0
+	github.com/kr/pretty v0.3.1
 	github.com/kylelemons/godebug v1.1.0
 	github.com/lestrrat-go/jwx v1.2.29
 	github.com/lmittmann/ppm v1.0.2
@@ -285,6 +286,7 @@ require (
 	github.com/kkHAIKE/contextcheck v1.1.5 // indirect
 	github.com/klauspost/compress v1.18.0 // indirect
 	github.com/klauspost/pgzip v1.2.6 // indirect
+	github.com/kr/text v0.2.0 // indirect
 	github.com/kulti/thelper v0.6.3 // indirect
 	github.com/kunwardeep/paralleltest v1.0.10 // indirect
 	github.com/kyoh86/exportloopref v0.1.11 // indirect
@@ -362,6 +364,7 @@ require (
 	github.com/rivo/uniseg v0.4.7 // indirect
 	github.com/robfig/cron v1.2.0 // indirect
 	github.com/robfig/cron/v3 v3.0.1 // indirect
+	github.com/rogpeppe/go-internal v1.14.1 // indirect
 	github.com/russross/blackfriday/v2 v2.1.0 // indirect
 	github.com/ryancurrah/gomodguard v1.3.5 // indirect
 	github.com/ryanrolds/sqlclosecheck v0.5.1 // indirect

go.sum

@@ -1135,6 +1135,7 @@ github.com/pion/webrtc/v3 v3.2.36 h1:RM/miAv0M4TrhhS7h2mcZXt44K68WmpVDkUOgz2l2l8
 github.com/pion/webrtc/v3 v3.2.36/go.mod h1:wWQz1PuKNSNK4VrJJNpPN3vZmKEi4zA6i2ynaQOlxIU=
 github.com/pkg/browser v0.0.0-20240102092130-5ac0b6a4141c h1:+mdjkGKdHQG3305AYmdv1U2eRNDiU2ErMBj1gwrq8eQ=
 github.com/pkg/browser v0.0.0-20240102092130-5ac0b6a4141c/go.mod h1:7rwL4CYBLnjLxUqIJNnCWiEdr3bn6IUYi15bNlnbCCU=
+github.com/pkg/diff v0.0.0-20210226163009-20ebb0f2a09e/go.mod h1:pJLUxLENpZxwdsKMEsNbx1VGcRFpLqf3715MtcvvzbA=
 github.com/pkg/errors v0.8.0/go.mod h1:bwawxfHBFNV+L2hUp1rHADufV3IMtnDRdf1r5NINEl0=
 github.com/pkg/errors v0.8.1/go.mod h1:bwawxfHBFNV+L2hUp1rHADufV3IMtnDRdf1r5NINEl0=
 github.com/pkg/errors v0.9.1 h1:FEBLx1zS214owpjy7qsBeixbURkuhQAwrK5UwLGTwt4=
@@ -1221,6 +1222,7 @@ github.com/rogpeppe/fastuuid v0.0.0-20150106093220-6724a57986af/go.mod h1:XWv6So
 github.com/rogpeppe/fastuuid v1.2.0/go.mod h1:jVj6XXZzXRy/MSR5jhDC/2q6DgLz+nrA6LYCDYWNEvQ=
 github.com/rogpeppe/go-internal v1.3.0/go.mod h1:M8bDsm7K2OlrFYOpmOWEs/qY81heoFRclV5y23lUDJ4=
 github.com/rogpeppe/go-internal v1.6.2/go.mod h1:xXDCJY+GAPziupqXw64V24skbSoqbTEfhy4qGm1nDQc=
+github.com/rogpeppe/go-internal v1.9.0/go.mod h1:WtVeX8xhTBvf0smdhujwtBcq4Qrzq/fJaraNFVN+nFs=
 github.com/rogpeppe/go-internal v1.14.1 h1:UQB4HGPB6osV0SQTLymcB4TgvyWu6ZyliaW0tI/otEQ=
 github.com/rogpeppe/go-internal v1.14.1/go.mod h1:MaRKkUm5W0goXpeCfT7UZI6fk/L7L7so1lCWt35ZSgc=
 github.com/rs/cors v1.7.0/go.mod h1:gFx+x8UowdsKA9AchylcLynDq+nNFfI8FkUZdN/jGCU=

motionplan/armplanning/cBiRRT.go

@@ -59,10 +59,11 @@ func newCBiRRTMotionPlanner(ctx context.Context, pc *planContext, psc *planSegme
 
 // only used for testin.
 func (mp *cBiRRTMotionPlanner) planForTest(ctx context.Context) ([]referenceframe.FrameSystemInputs, error) {
-	initMaps, err := initRRTSolutions(ctx, mp.psc)
+	initMaps, bgGen, err := initRRTSolutions(ctx, mp.psc)
 	if err != nil {
 		return nil, err
 	}
+	bgGen.StopAndWait() // Assume initial solutions are good enough.
 
 	x := []referenceframe.FrameSystemInputs{mp.psc.start}
 
@@ -71,7 +72,7 @@ func (mp *cBiRRTMotionPlanner) planForTest(ctx context.Context) ([]referencefram
 		return x, nil
 	}
 
-	solution, err := mp.rrtRunner(ctx, initMaps.maps)
+	solution, err := mp.rrtRunner(ctx, initMaps.maps, bgGen)
 	if err != nil {
 		return nil, err
 	}
@@ -84,21 +85,19 @@ func (mp *cBiRRTMotionPlanner) planForTest(ctx context.Context) ([]referencefram
 func (mp *cBiRRTMotionPlanner) rrtRunner(
 	ctx context.Context,
 	rrtMaps *rrtMaps,
+	bgSolutionGenerator *backgroundGenerator,
 ) (*rrtSolution, error) {
 	ctx, span := trace.StartSpan(ctx, "rrtRunner")
 	defer span.End()
 
-	mp.pc.logger.CDebugf(ctx, "starting cbirrt with start map len %d and goal map len %d\n", len(rrtMaps.startMap), len(rrtMaps.goalMap))
+	mp.pc.logger.CDebugf(ctx, "starting cbirrt with start map len %d and goal map len %d", len(rrtMaps.startMap), len(rrtMaps.goalMap))
 
 	// setup planner options
 	if mp.pc.planOpts == nil {
 		return nil, errNoPlannerOptions
 	}
 
-	_, cancel := context.WithCancel(ctx)
-	defer cancel()
 	startTime := time.Now()
-
 	var seed referenceframe.FrameSystemInputs
 
 	// initialize maps
@@ -109,8 +108,9 @@ func (mp *cBiRRTMotionPlanner) rrtRunner(
 			break
 		}
 	}
-	mp.pc.logger.CDebugf(ctx, "goal node: %v\n", rrtMaps.optNode.inputs)
-	mp.pc.logger.CDebugf(ctx, "start node: %v\n", seed)
+
+	mp.pc.logger.CInfof(ctx, "goal node: %v Name: %v Goal? %v", rrtMaps.optNode.inputs, rrtMaps.optNode.name, rrtMaps.optNode.goalNode)
+	mp.pc.logger.CInfof(ctx, "start node: %v", seed)
 	mp.pc.logger.Debug("DOF", mp.pc.lfs.dof)
 
 	interpConfig, err := referenceframe.InterpolateFS(mp.pc.fs, seed, rrtMaps.optNode.inputs, 0.5)
@@ -119,13 +119,24 @@ func (mp *cBiRRTMotionPlanner) rrtRunner(
 	}
 
 	target := newConfigurationNode(interpConfig)
+	mp.pc.logger.CInfof(ctx, "initial target. Name: %v Goal? %v", target.name, target.goalNode)
 
 	map1, map2 := rrtMaps.startMap, rrtMaps.goalMap
 	for i := 0; i < mp.pc.planOpts.PlanIter; i++ {
-		mp.pc.logger.CDebugf(ctx, "iteration: %d target: %v\n", i, target.inputs)
-		if ctx.Err() != nil {
-			mp.pc.logger.CDebugf(ctx, "CBiRRT timed out after %d iterations", i)
-			return &rrtSolution{maps: rrtMaps}, fmt.Errorf("cbirrt timeout %w", ctx.Err())
+		mp.pc.logger.CInfof(ctx, "iteration: %d target: %v target name: %v", i, target.inputs, target.name)
+
+	addNewGoals:
+		for {
+			select {
+			case <-ctx.Done():
+				mp.pc.logger.CDebugf(ctx, "CBiRRT timed out after %d iterations", i)
+				return &rrtSolution{maps: rrtMaps}, fmt.Errorf("cbirrt timeout %w", ctx.Err())
+			case newGoal := <-bgSolutionGenerator.newSolutionsCh:
+				mp.pc.logger.CInfof(ctx, "Added new goal while birrting. Goal: %v GoalName: %v", newGoal.inputs, newGoal.name)
+				rrtMaps.goalMap[newGoal] = nil
+			default:
+				break addNewGoals
+			}
 		}
 
 		tryExtend := func(target *node) (*node, *node) {
@@ -169,8 +180,7 @@ func (mp *cBiRRTMotionPlanner) rrtRunner(
 
 		// Solved!
 		if reachedDelta <= mp.pc.planOpts.InputIdentDist {
-			mp.pc.logger.CDebugf(ctx, "CBiRRT found solution after %d iterations in %v", i, time.Since(startTime))
-			cancel()
+			mp.pc.logger.CInfof(ctx, "CBiRRT found solution after %d iterations in %v", i, time.Since(startTime))
 			path := extractPath(rrtMaps.startMap, rrtMaps.goalMap, &nodePair{map1reached, map2reached}, true)
 			return &rrtSolution{steps: path, maps: rrtMaps}, nil
 		}
@@ -254,7 +264,7 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend(
 			doubled = false
 		}
 		// constrainNear will ensure path between oldNear and newNear satisfies constraints along the way
-		near = &node{inputs: newNear}
+		near = &node{name: int(nodeNameCounter.Add(1)), inputs: newNear}
 		rrtMap[near] = oldNear
 	}
 	return oldNear

motionplan/armplanning/cBiRRT_test.go

@@ -58,7 +58,8 @@ func TestSimpleLinearMotion(t *testing.T) {
 
 	mp, err := newCBiRRTMotionPlanner(ctx, pc, psc)
 	test.That(t, err, test.ShouldBeNil)
-	solutions, err := getSolutions(ctx, psc)
+	solutions, bgGen, err := getSolutions(ctx, psc)
+	bgGen.StopAndWait() // Original solutions must be good enough.
 	test.That(t, err, test.ShouldBeNil)
 
 	near1 := &node{inputs: referenceframe.FrameSystemInputs{m.Name(): home7}}

motionplan/armplanning/context.go

@@ -84,6 +84,8 @@ type planSegmentContext struct {
 
 	motionChains *motionChains
 	checker      *motionplan.ConstraintChecker
+
+	continueGeneratingSolutions bool
 }
 
 func newPlanSegmentContext(ctx context.Context, pc *planContext, start referenceframe.FrameSystemInputs,