-
Notifications
You must be signed in to change notification settings - Fork 110
/
rrtStarConnect.go
242 lines (213 loc) · 7.35 KB
/
rrtStarConnect.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
package motionplan
import (
"context"
"encoding/json"
"math"
"math/rand"
"time"
"github.com/edaniels/golog"
"go.viam.com/utils"
"go.viam.com/rdk/referenceframe"
"go.viam.com/rdk/spatialmath"
)
const (
// The number of nearest neighbors to consider when adding a new sample to the tree.
defaultNeighborhoodSize = 10
)
type rrtStarConnectOptions struct {
// The number of nearest neighbors to consider when adding a new sample to the tree
NeighborhoodSize int `json:"neighborhood_size"`
// Parameters common to all RRT implementations
*rrtOptions
}
// newRRTStarConnectOptions creates a struct controlling the running of a single invocation of the algorithm.
// All values are pre-set to reasonable defaults, but can be tweaked if needed.
func newRRTStarConnectOptions(planOpts *plannerOptions) (*rrtStarConnectOptions, error) {
algOpts := &rrtStarConnectOptions{
NeighborhoodSize: defaultNeighborhoodSize,
rrtOptions: newRRTOptions(planOpts),
}
// convert map to json
jsonString, err := json.Marshal(planOpts.extra)
if err != nil {
return nil, err
}
err = json.Unmarshal(jsonString, algOpts)
if err != nil {
return nil, err
}
return algOpts, nil
}
// rrtStarConnectMotionPlanner is an object able to asymptotically optimally path around obstacles to some goal for a given referenceframe.
// It uses the RRT*-Connect algorithm, Klemm et al 2015
// https://ieeexplore.ieee.org/document/7419012
type rrtStarConnectMotionPlanner struct{ *planner }
// NewRRTStarConnectMotionPlannerWithSeed creates a rrtStarConnectMotionPlanner object with a user specified random seed.
func newRRTStarConnectMotionPlanner(
frame referenceframe.Frame,
nCPU int,
seed *rand.Rand,
logger golog.Logger,
) (motionPlanner, error) {
planner, err := newPlanner(frame, nCPU, seed, logger)
if err != nil {
return nil, err
}
return &rrtStarConnectMotionPlanner{planner}, nil
}
func (mp *rrtStarConnectMotionPlanner) Plan(ctx context.Context,
goal spatialmath.Pose,
seed []referenceframe.Input,
planOpts *plannerOptions,
) ([][]referenceframe.Input, error) {
if planOpts == nil {
planOpts = newBasicPlannerOptions()
}
solutionChan := make(chan *rrtPlanReturn, 1)
utils.PanicCapturingGo(func() {
mp.rrtBackgroundRunner(ctx, goal, seed, &rrtParallelPlannerShared{planOpts, initRRTMaps(), nil, solutionChan})
})
select {
case <-ctx.Done():
return nil, ctx.Err()
case plan := <-solutionChan:
return plan.toInputs(), plan.err()
}
}
// rrtBackgroundRunner will execute the plan. Plan() will call rrtBackgroundRunner in a separate thread and wait for results.
// Separating this allows other things to call rrtBackgroundRunner in parallel allowing the thread-agnostic Plan to be accessible.
func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context,
goal spatialmath.Pose,
seed []referenceframe.Input,
rrt *rrtParallelPlannerShared,
) {
mp.logger.Debug("Starting RRT*")
defer close(rrt.solutionChan)
// setup planner options
if rrt.planOpts == nil {
rrt.planOpts = newBasicPlannerOptions()
}
algOpts, err := newRRTStarConnectOptions(rrt.planOpts)
if err != nil {
rrt.solutionChan <- &rrtPlanReturn{planerr: err}
return
}
mp.start = time.Now()
// get many potential end goals from IK solver
solutions, err := getSolutions(ctx, rrt.planOpts, mp.solver, goal, seed, mp.Frame(), mp.randseed.Int())
mp.logger.Debugf("RRT* found %d IK solutions", len(solutions))
if err != nil {
rrt.solutionChan <- &rrtPlanReturn{planerr: err}
return
}
// publish endpoint of plan if it is known
if rrt.planOpts.MaxSolutions == 1 && rrt.endpointPreview != nil {
mp.logger.Debug("RRT* found early final solution")
rrt.endpointPreview <- solutions[0]
}
// the smallest interpolated distance between the start and end input represents a lower bound on cost
_, optimalCost := rrt.planOpts.DistanceFunc(&ConstraintInput{StartInput: seed, EndInput: solutions[0].Q()})
// initialize maps
for i, solution := range solutions {
if i == 0 && mp.checkPath(rrt.planOpts, seed, solution.Q()) {
rrt.solutionChan <- &rrtPlanReturn{steps: []node{&basicNode{q: seed}, solution}}
mp.logger.Debug("RRT* could interpolate directly to goal")
return
}
rrt.rm.goalMap[newCostNode(solution.Q(), 0)] = nil
}
mp.logger.Debugf("RRT* failed to directly interpolate from %v to %v", seed, solutions[0].Q())
rrt.rm.startMap[newCostNode(seed, 0)] = nil
target := referenceframe.RandomFrameInputs(mp.frame, mp.randseed)
// Keep a list of the node pairs that have the same inputs
shared := make([]*nodePair, 0)
m1chan := make(chan node, 1)
m2chan := make(chan node, 1)
defer close(m1chan)
defer close(m2chan)
solved := false
for i := 0; i < algOpts.PlanIter; i++ {
select {
case <-ctx.Done():
// stop and return best path
if solved {
mp.logger.Debugf("RRT* timed out after %d iterations, returning best path", i)
rrt.solutionChan <- shortestPath(rrt.rm, shared, optimalCost)
} else {
mp.logger.Debugf("RRT* timed out after %d iterations, no path found", i)
rrt.solutionChan <- &rrtPlanReturn{planerr: ctx.Err(), rm: rrt.rm}
}
return
default:
}
// try to connect the target to map 1
utils.PanicCapturingGo(func() {
mp.extend(algOpts, rrt.rm.startMap, target, m1chan)
})
utils.PanicCapturingGo(func() {
mp.extend(algOpts, rrt.rm.goalMap, target, m2chan)
})
map1reached := <-m1chan
map2reached := <-m2chan
if map1reached != nil && map2reached != nil {
// target was added to both map
solved = true
shared = append(shared, &nodePair{map1reached, map2reached})
}
// get next sample, switch map pointers
target = referenceframe.RandomFrameInputs(mp.frame, mp.randseed)
}
mp.logger.Debug("RRT* exceeded max iter")
rrt.solutionChan <- shortestPath(rrt.rm, shared, optimalCost)
}
func (mp *rrtStarConnectMotionPlanner) extend(
algOpts *rrtStarConnectOptions,
tree rrtMap,
target []referenceframe.Input,
mchan chan node,
) {
if validTarget := mp.checkInputs(algOpts.planOpts, target); !validTarget {
mchan <- nil
return
}
// iterate over the k nearest neighbors and find the minimum cost to connect the target node to the tree
neighbors := kNearestNeighbors(algOpts.planOpts, tree, target, algOpts.NeighborhoodSize)
minCost := math.Inf(1)
minIndex := -1
for i, neighbor := range neighbors {
neighborNode := neighbor.node.(*costNode)
cost := neighborNode.cost + neighbor.dist
if mp.checkPath(algOpts.planOpts, neighborNode.Q(), target) {
minIndex = i
minCost = cost
// Neighbors are returned ordered by their costs. The first valid one we find is best, so break here.
break
}
}
// add new node to tree as a child of the minimum cost neighbor node if it was reachable
if minIndex == -1 {
mchan <- nil
return
}
targetNode := newCostNode(target, minCost)
tree[targetNode] = neighbors[minIndex].node
// rewire the tree
for i, neighbor := range neighbors {
// dont need to try to rewire minIndex, so skip it
if i == minIndex {
continue
}
// check to see if a shortcut is possible, and rewire the node if it is
neighborNode := neighbor.node.(*costNode)
_, connectionCost := algOpts.planOpts.DistanceFunc(&ConstraintInput{
StartInput: neighborNode.Q(),
EndInput: targetNode.Q(),
})
cost := connectionCost + targetNode.cost
if cost < neighborNode.cost && mp.checkPath(algOpts.planOpts, target, neighborNode.Q()) {
neighborNode.cost = cost
tree[neighborNode] = targetNode
}
}
mchan <- targetNode
}