cache frame system kinematics and use for seeding ik

Author: erh

etc/test.sh

@@ -25,7 +25,7 @@ if test -n "$GITHUB_RUN_ID"; then
 fi
 
 # We run analyzetests on every run, pass or fail. We only run analyzecoverage when all tests passed.
-PION_LOG_WARN=webrtc,datachannel,sctp gotestsum --format $FORMAT $LOGFILE -- -tags=no_skip $RACE $COVER $TEST_TARGET
+PION_LOG_WARN=webrtc,datachannel,sctp gotestsum --format $FORMAT $LOGFILE -- -tags=no_skip -timeout 30m $RACE $COVER $TEST_TARGET
 SUCCESS=$?
 
 if [[ $RACE != "" ]]; then

motionplan/armplanning/cBiRRT.go

@@ -293,7 +293,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
 			return nil
 		}
 
-		solutions, err := ik.DoSolve(ctx, mp.fastGradDescent, mp.pc.linearizeFSmetric(mp.psc.checker.PathMetric()), linearSeed, .25)
+		solutions, err := ik.DoSolve(ctx, mp.fastGradDescent, mp.pc.linearizeFSmetric(mp.psc.checker.PathMetric()), [][]float64{linearSeed}, .25)
 		if err != nil {
 			mp.pc.logger.Debugf("constrainNear fail (DoSolve): %v", err)
 			return nil

motionplan/armplanning/node.go

@@ -107,8 +107,9 @@ type solutionSolvingState struct {
 	psc          *planSegmentContext
 	maxSolutions int
 
-	seed              referenceframe.FrameSystemInputs
-	linearSeed        []float64
+	seeds       []referenceframe.FrameSystemInputs
+	linearSeeds [][]float64
+
 	moving, nonmoving []string
 
 	ratios   []float64
@@ -130,7 +131,7 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er
 
 	sss := &solutionSolvingState{
 		psc:                  psc,
-		seed:                 psc.start,
+		seeds:                []referenceframe.FrameSystemInputs{psc.start},
 		solutions:            []*node{},
 		failures:             newIkConstraintError(psc.pc.fs, psc.checker),
 		startTime:            time.Now(),
@@ -144,11 +145,32 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er
 		sss.maxSolutions = defaultSolutionsToSeed
 	}
 
-	psc.pc.logger.Debugf("psc.start -> seed: \n%v\n%v", psc.start, sss.seed)
-	sss.linearSeed, err = psc.pc.lfs.mapToSlice(sss.seed)
+	ls, err := psc.pc.lfs.mapToSlice(psc.start)
 	if err != nil {
 		return nil, err
 	}
+	sss.linearSeeds = [][]float64{ls}
+
+	if len(psc.pc.lfs.dof) <= 6 { // TODO - remove the limit
+		ssc, err := smartSeed(psc.pc.fs, psc.pc.logger)
+		if err != nil {
+			return nil, fmt.Errorf("cannot create smartSeeder: %w", err)
+		}
+
+		altSeeds, err := ssc.findSeeds(psc.goal, psc.start, psc.pc.logger)
+		if err != nil {
+			psc.pc.logger.Warnf("findSeeds failed, ignoring: %v", err)
+		}
+		for _, s := range altSeeds {
+			ls, err := psc.pc.lfs.mapToSlice(s)
+			if err != nil {
+				psc.pc.logger.Warnf("mapToSlice failed? %v", err)
+				continue
+			}
+			sss.seeds = append(sss.seeds, s)
+			sss.linearSeeds = append(sss.linearSeeds, ls)
+		}
+	}
 
 	sss.moving, sss.nonmoving = sss.psc.motionChains.framesFilteredByMovingAndNonmoving()
 
@@ -161,12 +183,12 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er
 }
 
 func (sss *solutionSolvingState) computeGoodCost(goal referenceframe.FrameSystemPoses) error {
-	sss.ratios = sss.psc.pc.lfs.inputChangeRatio(sss.psc.motionChains, sss.seed,
+	sss.ratios = sss.psc.pc.lfs.inputChangeRatio(sss.psc.motionChains, sss.seeds[0], /* maybe use the best one? */
 		sss.psc.pc.planOpts.getGoalMetric(goal), sss.psc.pc.logger)
 
 	adjusted := []float64{}
 	for idx, r := range sss.ratios {
-		adjusted = append(adjusted, sss.psc.pc.lfs.jog(idx, sss.linearSeed[idx], r))
+		adjusted = append(adjusted, sss.psc.pc.lfs.jog(idx, sss.linearSeeds[0][idx] /* match above when we change */, r))
 	}
 	step, err := sss.psc.pc.lfs.sliceToMap(adjusted)
 	if err != nil {
@@ -314,6 +336,9 @@ func (sss *solutionSolvingState) shouldStopEarly() bool {
 	if sss.bestScoreNoProblem < sss.goodCost/20 {
 		multiple = 0
 		minMillis = 10
+	} else if sss.bestScoreNoProblem < sss.goodCost/15 {
+		multiple = 1
+		minMillis = 15
 	} else if sss.bestScoreNoProblem < sss.goodCost/10 {
 		multiple = 0
 		minMillis = 20
@@ -384,7 +409,7 @@ func getSolutions(ctx context.Context, psc *planSegmentContext) ([]*node, error)
 	utils.PanicCapturingGo(func() {
 		// This channel close doubles as signaling that the goroutine has exited.
 		defer close(solutionGen)
-		_, err := solver.Solve(ctxWithCancel, solutionGen, solvingState.linearSeed, solvingState.ratios, minFunc, psc.pc.randseed.Int())
+		_, err := solver.Solve(ctxWithCancel, solutionGen, solvingState.linearSeeds, solvingState.ratios, minFunc, psc.pc.randseed.Int())
 		if err != nil {
 			solveErrorLock.Lock()
 			solveError = err

motionplan/armplanning/real_test.go

@@ -153,22 +153,8 @@ func TestPirouette(t *testing.T) {
 	armKinematics, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/kinematics/ur5e.json"), armName)
 	test.That(t, err, test.ShouldBeNil)
 
-	idealJointValues := [][]referenceframe.Input{
-		{0 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{30 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{60 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{90 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{120 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{150 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{180 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{180 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{150 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{120 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{90 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{60 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{30 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-		{0 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0},
-	}
+	idealJointValues := pirIdealJointValues
+
 	// the only change here is in joint 0 in increments of 30, while all the other joints are kept at a constant value
 	// below is change in joint 0 in degrees:
 	// 0 -> 30 -> 60 -> 90 -> 120 -> 150 -> 180 -> 180 -> 150 -> 120 -> 90 -> 60 -> 30 -> 0