remove rrtstar (viamrobotics#5242)

Author: erh

motionplan/armplanning/motion_planner.go

@@ -214,56 +214,6 @@ func (mp *planner) getScoringFunction() motionplan.SegmentFSMetric {
 	return mp.scoringFunction
 }
 
-// smoothPath will try to naively smooth the path by picking points partway between waypoints and seeing if it can interpolate
-// directly between them. This will significantly improve paths from RRT*, as it will shortcut the randomly-selected configurations.
-// This will only ever improve paths (or leave them untouched), and runs very quickly.
-func (mp *planner) smoothPath(ctx context.Context, path []node) []node {
-	mp.logger.CDebugf(ctx, "running simple smoother on path of len %d", len(path))
-	if mp.planOpts == nil {
-		mp.logger.CDebug(ctx, "nil opts, cannot shortcut")
-		return path
-	}
-	if len(path) <= 2 {
-		mp.logger.CDebug(ctx, "path too short, cannot shortcut")
-		return path
-	}
-
-	// Randomly pick which quarter of motion to check from; this increases flexibility of smoothing.
-	waypoints := []float64{0.25, 0.5, 0.75}
-
-	for i := 0; i < mp.planOpts.SmoothIter; i++ {
-		select {
-		case <-ctx.Done():
-			return path
-		default:
-		}
-		// get start node of first edge. Cannot be either the last or second-to-last node.
-		// Intn will return an int in the half-open interval half-open interval [0,n)
-		firstEdge := mp.randseed.Intn(len(path) - 2)
-		secondEdge := firstEdge + 1 + mp.randseed.Intn((len(path)-2)-firstEdge)
-
-		// Use the frame system to interpolate between configurations
-		wayPoint1, err := referenceframe.InterpolateFS(mp.fs, path[firstEdge].Q(), path[firstEdge+1].Q(), waypoints[mp.randseed.Intn(3)])
-		if err != nil {
-			return path
-		}
-		wayPoint2, err := referenceframe.InterpolateFS(mp.fs, path[secondEdge].Q(), path[secondEdge+1].Q(), waypoints[mp.randseed.Intn(3)])
-		if err != nil {
-			return path
-		}
-
-		if mp.checkPath(wayPoint1, wayPoint2) {
-			newpath := []node{}
-			newpath = append(newpath, path[:firstEdge+1]...)
-			newpath = append(newpath, newConfigurationNode(wayPoint1), newConfigurationNode(wayPoint2))
-			// have to split this up due to go compiler quirk where elipses operator can't be mixed with other vars in append
-			newpath = append(newpath, path[secondEdge+1:]...)
-			path = newpath
-		}
-	}
-	return path
-}
-
 type solutionSolvingState struct {
 	solutions         map[float64]referenceframe.FrameSystemInputs
 	failures          map[string]int // A map keeping track of which constraints fail

motionplan/armplanning/motion_planner_test.go

@@ -48,7 +48,6 @@ type planConfigConstructor func(logger logging.Logger) (*planConfig, error)
 func TestUnconstrainedMotion(t *testing.T) {
 	t.Parallel()
 	planners := []plannerConstructor{
-		newRRTStarConnectMotionPlanner,
 		newCBiRRTMotionPlanner,
 	}
 	testCases := []struct {

motionplan/armplanning/nearest_neighbor.go

@@ -5,7 +5,6 @@ package armplanning
 import (
 	"context"
 	"math"
-	"sort"
 
 	"go.viam.com/utils"
 )
@@ -25,40 +24,6 @@ type neighbor struct {
 	node node
 }
 
-func kNearestNeighbors(tree rrtMap, target node, neighborhoodSize int, nodeDistanceFunc NodeDistanceMetric) []*neighbor {
-	kNeighbors := neighborhoodSize
-	if neighborhoodSize > len(tree) {
-		kNeighbors = len(tree)
-	}
-
-	allCosts := make([]*neighbor, 0)
-	for rrtnode := range tree {
-		dist := nodeDistanceFunc(target, rrtnode)
-		allCosts = append(allCosts, &neighbor{dist: dist, node: rrtnode})
-	}
-	// sort neighbors by their distance to target first so that first nearest neighbor isn't always the start node of tree
-	sort.Slice(allCosts, func(i, j int) bool {
-		if !math.IsNaN(allCosts[i].node.Cost()) {
-			if !math.IsNaN(allCosts[j].node.Cost()) {
-				return allCosts[i].dist < allCosts[j].dist
-			}
-		}
-		return allCosts[i].dist < allCosts[j].dist
-	})
-	allCosts = allCosts[:kNeighbors]
-	// sort k nearest distance neighbors by "total cost to target" metric so that target's nearest neighbor
-	// provides the smallest cost path from start node to target
-	sort.Slice(allCosts, func(i, j int) bool {
-		if !math.IsNaN(allCosts[i].node.Cost()) {
-			if !math.IsNaN(allCosts[j].node.Cost()) {
-				return (allCosts[i].dist + allCosts[i].node.Cost()) < (allCosts[j].dist + allCosts[j].node.Cost())
-			}
-		}
-		return allCosts[i].dist < allCosts[j].dist
-	})
-	return allCosts
-}
-
 // Can return `nil` when the context is canceled during processing.
 func (nm *neighborManager) nearestNeighbor(
 	ctx context.Context,

motionplan/armplanning/node.go

@@ -91,18 +91,6 @@ func (n *basicNode) SetCorner(corner bool) {
 // TODO(rb): in the future we might think about making this into a list of nodes.
 type nodePair struct{ a, b node }
 
-func (np *nodePair) sumCosts() float64 {
-	aCost := np.a.Cost()
-	if math.IsNaN(aCost) {
-		return 0
-	}
-	bCost := np.b.Cost()
-	if math.IsNaN(bCost) {
-		return 0
-	}
-	return aCost + bCost
-}
-
 func extractPath(startMap, goalMap map[node]node, pair *nodePair, matched bool) []node {
 	// need to figure out which of the two nodes is in the start map
 	var startReached, goalReached node

motionplan/armplanning/planning_algorithms.go

@@ -16,8 +16,6 @@ const (
 	// CBiRRT indicates that a CBiRRTMotionPlanner should be used. This is currently the
 	// default motion planner.
 	CBiRRT PlanningAlgorithm = "cbirrt"
-	// RRTStar indicates that an RRTStarConnectMotionPlanner should be used.
-	RRTStar PlanningAlgorithm = "rrtstar"
 	// TPSpace indicates that TPSpaceMotionPlanner should be used.
 	TPSpace PlanningAlgorithm = "tpspace"
 	// UnspecifiedAlgorithm indicates that the use of our motion planning will accept whatever defaults the package
@@ -28,9 +26,8 @@ const (
 // AlgorithmSettings is a polymorphic representation of motion planning algorithms and their parameters. The `Algorithm`
 // should correlate with the available options (e.g. if `Algorithm` us CBiRRT, RRTStarOpts should be nil and CBirrtOpts should not).
 type AlgorithmSettings struct {
-	Algorithm   PlanningAlgorithm      `json:"algorithm"`
-	CBirrtOpts  *cbirrtOptions         `json:"cbirrt_settings"`
-	RRTStarOpts *rrtStarConnectOptions `json:"rrtstar_settings"`
+	Algorithm  PlanningAlgorithm `json:"algorithm"`
+	CBirrtOpts *cbirrtOptions    `json:"cbirrt_settings"`
 }
 
 // move back to cBiRRT.go when motionplan is taken out of RDK.
@@ -42,15 +39,6 @@ type cbirrtOptions struct {
 	qstep map[string][]float64
 }
 
-// move back to rrtStarConnect.go when motionplan is taken out of RDK.
-type rrtStarConnectOptions struct {
-	// The number of nearest neighbors to consider when adding a new sample to the tree
-	NeighborhoodSize int `json:"neighborhood_size"`
-
-	// This is how far rrtStarConnect will try to extend the map towards a goal per-step
-	qstep map[string][]float64
-}
-
 type plannerConstructor func(
 	*referenceframe.FrameSystem,
 	*rand.Rand,
@@ -71,8 +59,6 @@ func newMotionPlanner(
 	switch opt.PlanningAlgorithm() {
 	case CBiRRT:
 		return newCBiRRTMotionPlanner(fs, seed, logger, opt, constraintHandler, chains)
-	case RRTStar:
-		return newRRTStarConnectMotionPlanner(fs, seed, logger, opt, constraintHandler, chains)
 	case TPSpace:
 		return newTPSpaceMotionPlanner(fs, seed, logger, opt, constraintHandler, chains)
 	case UnspecifiedAlgorithm:

motionplan/armplanning/rrt.go

@@ -105,21 +105,6 @@ func initRRTSolutions(ctx context.Context, wp atomicWaypoint) *rrtSolution {
 	return rrt
 }
 
-func shortestPath(maps *rrtMaps, nodePairs []*nodePair) *rrtSolution {
-	if len(nodePairs) == 0 {
-		return &rrtSolution{err: errPlannerFailed, maps: maps}
-	}
-	minIdx := 0
-	minDist := nodePairs[0].sumCosts()
-	for i := 1; i < len(nodePairs); i++ {
-		if dist := nodePairs[i].sumCosts(); dist < minDist {
-			minDist = dist
-			minIdx = i
-		}
-	}
-	return &rrtSolution{steps: extractPath(maps.startMap, maps.goalMap, nodePairs[minIdx], true), maps: maps}
-}
-
 type rrtPlan struct {
 	motionplan.SimplePlan