Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

RSDK-8852 migrate motionplan to work on framesystems rather than individual frames #4558

Closed
wants to merge 9 commits into from
Closed

RSDK-8852 migrate motionplan to work on framesystems rather than individual frames #4558

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
7fdbfdd
Speedup spatialmath, update IK to be more generic
biotinker Sep 30, 2024
8b91ab1
wip
biotinker Oct 2, 2024
1edf4bc
more wip, test-pass IK
biotinker Oct 3, 2024
20a19d4
Merge branch 'main' into 20240924_RSDK-8852-migrate-motionplan-to-wor…
biotinker Oct 14, 2024
78bd9e4
wip
biotinker Oct 30, 2024
5d51171
Merge branch 'main' into 20240924_RSDK-8852-migrate-motionplan-to-wor…
biotinker Oct 30, 2024
5688dd3
wip
biotinker Nov 6, 2024
6a8a3ca
All work to update motionplannign to work on framesystems
biotinker Nov 15, 2024
cf82763
Assorted small test fixes
biotinker Nov 15, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
182 changes: 94 additions & 88 deletions motionplan/cBiRRT.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@ import (
"go.viam.com/rdk/logging"
"go.viam.com/rdk/motionplan/ik"
"go.viam.com/rdk/referenceframe"
"go.viam.com/rdk/spatialmath"
)

const (
Expand All @@ -30,11 +29,14 @@ const (
type cbirrtOptions struct {
// Number of IK solutions with which to seed the goal side of the bidirectional tree.
SolutionsToSeed int `json:"solutions_to_seed"`

// This is how far cbirrt will try to extend the map towards a goal per-step. Determined from FrameStep
qstep map[string][]float64
}

// newCbirrtOptions creates a struct controlling the running of a single invocation of cbirrt. All values are pre-set to reasonable
// defaults, but can be tweaked if needed.
func newCbirrtOptions(planOpts *plannerOptions) (*cbirrtOptions, error) {
func newCbirrtOptions(planOpts *plannerOptions, lfs *linearizedFrameSystem) (*cbirrtOptions, error) {
algOpts := &cbirrtOptions{
SolutionsToSeed: defaultSolutionsToSeed,
}
Expand All @@ -47,6 +49,7 @@ func newCbirrtOptions(planOpts *plannerOptions) (*cbirrtOptions, error) {
if err != nil {
return nil, err
}
algOpts.qstep = getFrameSteps(lfs, defaultFrameStep)

return algOpts, nil
}
Expand All @@ -62,24 +65,24 @@ type cBiRRTMotionPlanner struct {

// newCBiRRTMotionPlannerWithSeed creates a cBiRRTMotionPlanner object with a user specified random seed.
func newCBiRRTMotionPlanner(
frame referenceframe.Frame,
fss referenceframe.FrameSystem,
seed *rand.Rand,
logger logging.Logger,
opt *plannerOptions,
) (motionPlanner, error) {
if opt == nil {
return nil, errNoPlannerOptions
}
mp, err := newPlanner(frame, seed, logger, opt)
mp, err := newPlanner(fss, seed, logger, opt)
if err != nil {
return nil, err
}
// nlopt should try only once
nlopt, err := ik.CreateNloptSolver(frame.DoF(), logger, 1, true, true)
nlopt, err := ik.CreateNloptSolver(mp.lfs.dof, logger, 1, true, true)
if err != nil {
return nil, err
}
algOpts, err := newCbirrtOptions(opt)
algOpts, err := newCbirrtOptions(opt, mp.lfs)
if err != nil {
return nil, err
}
Expand All @@ -90,8 +93,8 @@ func newCBiRRTMotionPlanner(
}, nil
}

func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, goal spatialmath.Pose, seed []referenceframe.Input) ([]node, error) {
mp.planOpts.SetGoal(goal)
func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, goal PathStep, seed map[string][]referenceframe.Input) ([]node, error) {
mp.planOpts.setGoal(goal)
solutionChan := make(chan *rrtSolution, 1)
utils.PanicCapturingGo(func() {
mp.rrtBackgroundRunner(ctx, seed, &rrtParallelPlannerShared{nil, nil, solutionChan})
Expand All @@ -107,7 +110,7 @@ func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, goal spatialmath.Pose,
// Separating this allows other things to call rrtBackgroundRunner in parallel allowing the thread-agnostic Plan to be accessible.
func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
ctx context.Context,
seed []referenceframe.Input,
seed map[string][]referenceframe.Input,
rrt *rrtParallelPlannerShared,
) {
defer close(rrt.solutionChan)
Expand All @@ -134,7 +137,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
rrt.maps = planSeed.maps
}
mp.logger.CInfof(ctx, "goal node: %v\n", rrt.maps.optNode.Q())
interpConfig, err := mp.frame.Interpolate(seed, rrt.maps.optNode.Q(), 0.5)
interpConfig, err := referenceframe.InterpolateFS(mp.fss, seed, rrt.maps.optNode.Q(), 0.5)
if err != nil {
rrt.solutionChan <- &rrtSolution{err: err}
return
Expand All @@ -148,18 +151,18 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
defer close(m1chan)
defer close(m2chan)

seedPos, err := mp.frame.Transform(seed)
if err != nil {
rrt.solutionChan <- &rrtSolution{err: err}
return
}
//~ seedPos, err := mp.frame.Transform(seed)
//~ if err != nil {
//~ rrt.solutionChan <- &rrtSolution{err: err}
//~ return
//~ }

mp.logger.CDebugf(ctx,
"running CBiRRT from start pose %v with start map of size %d and goal map of size %d",
spatialmath.PoseToProtobuf(seedPos),
len(rrt.maps.startMap),
len(rrt.maps.goalMap),
)
//~ mp.logger.CDebugf(ctx,
//~ "running CBiRRT from start pose %v with start map of size %d and goal map of size %d",
//~ spatialmath.PoseToProtobuf(seedPos),
//~ len(rrt.maps.startMap),
//~ len(rrt.maps.goalMap),
//~ )

for i := 0; i < mp.planOpts.PlanIter; i++ {
select {
Expand Down Expand Up @@ -216,11 +219,11 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
return
}

reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()})
reachedDelta := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()})

// Second iteration; extend maps 1 and 2 towards the halfway point between where they reached
if reachedDelta > mp.planOpts.JointSolveDist {
targetConf, err := mp.frame.Interpolate(map1reached.Q(), map2reached.Q(), 0.5)
if reachedDelta > mp.planOpts.InputIdentDist {
targetConf, err := referenceframe.InterpolateFS(mp.fss, map1reached.Q(), map2reached.Q(), 0.5)
if err != nil {
rrt.solutionChan <- &rrtSolution{err: err, maps: rrt.maps}
return
Expand All @@ -231,11 +234,11 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
rrt.solutionChan <- &rrtSolution{err: err, maps: rrt.maps}
return
}
reachedDelta = mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()})
reachedDelta = mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()})
}

// Solved!
if reachedDelta <= mp.planOpts.JointSolveDist {
if reachedDelta <= mp.planOpts.InputIdentDist {
mp.logger.CDebugf(ctx, "CBiRRT found solution after %d iterations", i)
cancel()
path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{map1reached, map2reached}, true)
Expand Down Expand Up @@ -264,8 +267,16 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend(
mchan chan node,
) {
// Allow qstep to be doubled as a means to escape from configurations which gradient descend to their seed
qstep := make([]float64, len(mp.planOpts.qstep))
copy(qstep, mp.planOpts.qstep)
deepCopyQstep := func() map[string][]float64 {
qstep := map[string][]float64{}
for fName, fStep := range mp.algOpts.qstep {
newStep := make([]float64, len(fStep))
copy(newStep, fStep)
qstep[fName] = newStep
}
return qstep
}
qstep := deepCopyQstep()
doubled := false

oldNear := near
Expand All @@ -283,10 +294,10 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend(
default:
}

dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: near.Q(), EndConfiguration: target.Q()})
oldDist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: oldNear.Q(), EndConfiguration: target.Q()})
dist := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: near.Q(), EndConfiguration: target.Q()})
oldDist := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: oldNear.Q(), EndConfiguration: target.Q()})
switch {
case dist < mp.planOpts.JointSolveDist:
case dist < mp.planOpts.InputIdentDist:
mchan <- near
return
case dist > oldDist:
Expand All @@ -296,20 +307,22 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend(

oldNear = near

newNear := fixedStepInterpolation(near, target, mp.planOpts.qstep)
newNear := fixedStepInterpolation(near, target, mp.algOpts.qstep)
// Check whether newNear meets constraints, and if not, update it to a configuration that does meet constraints (or nil)
newNear = mp.constrainNear(ctx, randseed, oldNear.Q(), newNear)

if newNear != nil {
nearDist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: oldNear.Q(), EndConfiguration: newNear})
if nearDist < math.Pow(mp.planOpts.JointSolveDist, 3) {
nearDist := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: oldNear.Q(), EndConfiguration: newNear})
if nearDist < math.Pow(mp.planOpts.InputIdentDist, 3) {
if !doubled {
doubled = true
// Check if doubling qstep will allow escape from the identical configuration
// If not, we terminate and return.
// If so, qstep will be reset to its original value after the rescue.
for i, q := range qstep {
qstep[i] = q * 2.0
for f, frameQ := range qstep {
for i, q := range frameQ {
qstep[f][i] = q * 2.0
}
}
continue
}
Expand All @@ -319,7 +332,7 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend(
return
}
if doubled {
copy(qstep, mp.planOpts.qstep)
qstep = deepCopyQstep()
doubled = false
}
// constrainNear will ensure path between oldNear and newNear satisfies constraints along the way
Expand All @@ -339,49 +352,34 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
ctx context.Context,
randseed *rand.Rand,
seedInputs,
target []referenceframe.Input,
) []referenceframe.Input {
target map[string][]referenceframe.Input,
) map[string][]referenceframe.Input {
for i := 0; i < maxNearIter; i++ {
select {
case <-ctx.Done():
return nil
default:
}

seedPos, err := mp.frame.Transform(seedInputs)
if err != nil {
return nil
}
goalPos, err := mp.frame.Transform(target)
if err != nil {
return nil
}

newArc := &ik.Segment{
StartPosition: seedPos,
EndPosition: goalPos,
newArc := &ik.SegmentFS{
StartConfiguration: seedInputs,
EndConfiguration: target,
Frame: mp.frame,
FS: mp.fss,
}

// Check if the arc of "seedInputs" to "target" is valid
ok, _ := mp.planOpts.CheckSegmentAndStateValidity(newArc, mp.planOpts.Resolution)
ok, _ := mp.planOpts.CheckSegmentAndStateValidityFS(newArc, mp.planOpts.Resolution)
if ok {
return target
}
solutionGen := make(chan *ik.Solution, 1)
linearSeed, err := mp.lfs.mapToSlice(target)
if err != nil {
return nil
}

// Spawn the IK solver to generate solutions until done
err = ik.SolveMetric(
ctx,
mp.fastGradDescent,
mp.frame,
solutionGen,
target,
mp.planOpts.pathMetric,
randseed.Int(),
mp.logger,
)
err = mp.fastGradDescent.Solve(ctx, solutionGen, linearSeed, mp.linearizeFSmetric(mp.planOpts.pathMetric), randseed.Int())
// We should have zero or one solutions
var solved *ik.Solution
select {
Expand All @@ -392,21 +390,25 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
if err != nil || solved == nil {
return nil
}
solutionMap, err := mp.lfs.sliceToMap(solved.Configuration)
if err != nil {
return nil
}

ok, failpos := mp.planOpts.CheckSegmentAndStateValidity(
&ik.Segment{
ok, failpos := mp.planOpts.CheckSegmentAndStateValidityFS(
&ik.SegmentFS{
StartConfiguration: seedInputs,
EndConfiguration: referenceframe.FloatsToInputs(solved.Configuration),
Frame: mp.frame,
EndConfiguration: solutionMap,
FS: mp.fss,
},
mp.planOpts.Resolution,
)
if ok {
return referenceframe.FloatsToInputs(solved.Configuration)
return solutionMap
}
if failpos != nil {
dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: target, EndConfiguration: failpos.EndConfiguration})
if dist > mp.planOpts.JointSolveDist {
dist := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: target, EndConfiguration: failpos.EndConfiguration})
if dist > mp.planOpts.InputIdentDist {
// If we have a first failing position, and that target is updating (no infinite loop), then recurse
seedInputs = failpos.StartConfiguration
target = failpos.EndConfiguration
Expand Down Expand Up @@ -463,8 +465,8 @@ func (mp *cBiRRTMotionPlanner) smoothPath(ctx context.Context, inputSteps []node
// Note this could technically replace paths with "longer" paths i.e. with more waypoints.
// However, smoothed paths are invariably more intuitive and smooth, and lend themselves to future shortening,
// so we allow elongation here.
dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: inputSteps[i].Q(), EndConfiguration: reached.Q()})
if dist < mp.planOpts.JointSolveDist {
dist := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: inputSteps[i].Q(), EndConfiguration: reached.Q()})
if dist < mp.planOpts.InputIdentDist {
for _, hitCorner := range hitCorners {
hitCorner.SetCorner(false)
}
Expand All @@ -486,22 +488,26 @@ func (mp *cBiRRTMotionPlanner) smoothPath(ctx context.Context, inputSteps []node

// getFrameSteps will return a slice of positive values representing the largest amount a particular DOF of a frame should
// move in any given step. The second argument is a float describing the percentage of the total movement.
func getFrameSteps(f referenceframe.Frame, percentTotalMovement float64) []float64 {
dof := f.DoF()
pos := make([]float64, len(dof))
for i, lim := range dof {
l, u := lim.Min, lim.Max

// Default to [-999,999] as range if limits are infinite
if l == math.Inf(-1) {
l = -999
}
if u == math.Inf(1) {
u = 999
}
func getFrameSteps(lfs *linearizedFrameSystem, percentTotalMovement float64) map[string][]float64 {
frameQstep := map[string][]float64{}
for _, f := range lfs.frames {
dof := f.DoF()
pos := make([]float64, len(dof))
for i, lim := range dof {
l, u := lim.Min, lim.Max

// Default to [-999,999] as range if limits are infinite
if l == math.Inf(-1) {
l = -999
}
if u == math.Inf(1) {
u = 999
}

jRange := math.Abs(u - l)
pos[i] = jRange * percentTotalMovement
jRange := math.Abs(u - l)
pos[i] = jRange * percentTotalMovement
}
frameQstep[f.Name()] = pos
}
return pos
return frameQstep
}
Loading
Loading