From fe08a378f0d5ea7aed8289f6db33e0e560887979 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Sat, 8 Nov 2025 00:17:02 -0500 Subject: [PATCH] try going halfway on cbirrt before going down full path --- motionplan/armplanning/plan_manager.go | 86 +++++++++----------------- motionplan/constraint_checker.go | 34 ++++++---- 2 files changed, 50 insertions(+), 70 deletions(-) diff --git a/motionplan/armplanning/plan_manager.go b/motionplan/armplanning/plan_manager.go index eec1b194adc..85491295351 100644 --- a/motionplan/armplanning/plan_manager.go +++ b/motionplan/armplanning/plan_manager.go @@ -143,27 +143,6 @@ func (pm *planManager) planToDirectJoints( return nil, fmt.Errorf("want to go to specific joint config but it is invalid: %w", err) } - if false { // true cartesian half - // TODO(eliot): finish me - startPoses, err := start.ComputePoses(pm.pc.fs) - if err != nil { - return nil, err - } - - mid := interp(startPoses, goalPoses, .5) - - pm.logger.Infof("foo things\n\t%v\n\t%v\n\t%v", startPoses, mid, goalPoses) - - err = pm.foo(ctx, start, mid) - if err != nil { - pm.logger.Infof("foo failed: %v", err) - } else { - panic(2) - } - - // panic(1) - } - pathPlanner, err := newCBiRRTMotionPlanner(ctx, pm.pc, psc) if err != nil { return nil, err @@ -209,6 +188,19 @@ func (pm *planManager) planSingleGoal( return planSeed.steps, nil } + midJoints, err := pm.useMidPointBeforeSearch(ctx, psc, planSeed.maps.optNode.inputs) + if err != nil { + return nil, err + } + + if midJoints != nil { + psc, err = newPlanSegmentContext(ctx, pm.pc, midJoints, goal) + if err != nil { + return nil, err + } + planSeed.maps.startMap = rrtMap{&node{inputs: midJoints}: nil} + } + pathPlanner, err := newCBiRRTMotionPlanner(ctx, pm.pc, psc) if err != nil { return nil, err @@ -218,6 +210,9 @@ func (pm *planManager) planSingleGoal( if err != nil { return nil, err } + if midJoints != nil { + finalSteps.steps = append([]*referenceframe.LinearInputs{start}, finalSteps.steps...) + } finalSteps.steps = smoothPath(ctx, psc, finalSteps.steps) return finalSteps.steps, nil } @@ -335,47 +330,22 @@ func initRRTSolutions(ctx context.Context, psc *planSegmentContext) (*rrtSolutio return rrt, nil } -func interp(start, end referenceframe.FrameSystemPoses, delta float64) referenceframe.FrameSystemPoses { - mid := referenceframe.FrameSystemPoses{} - - for k, s := range start { - e, ok := end[k] - if !ok { - mid[k] = s - continue - } - if s.Parent() != e.Parent() { - panic("eliottttt") - } - m := spatialmath.Interpolate(s.Pose(), e.Pose(), delta) - mid[k] = referenceframe.NewPoseInFrame(s.Parent(), m) - } - return mid -} - -func (pm *planManager) foo(ctx context.Context, start *referenceframe.LinearInputs, goal referenceframe.FrameSystemPoses) error { - psc, err := newPlanSegmentContext(ctx, pm.pc, start, goal) - if err != nil { - return err - } +func (pm *planManager) useMidPointBeforeSearch(ctx context.Context, + psc *planSegmentContext, + goal *referenceframe.LinearInputs, +) (*referenceframe.LinearInputs, error) { + ctx, span := trace.StartSpan(ctx, "quickReroute") + defer span.End() - planSeed, err := initRRTSolutions(ctx, psc) + midJoints, err := motionplan.InterpLinear(pm.pc.fs, psc.start, goal, .5) if err != nil { - return err - } - - if planSeed.steps == nil { - return fmt.Errorf("no steps") - } - - if len(planSeed.steps) != 1 { - return fmt.Errorf("steps odd %d", len(planSeed.steps)) + return nil, err } - err = psc.checkPath(ctx, start, planSeed.steps[0]) - if err != nil { - return err + if psc.checkPath(ctx, psc.start, midJoints) == nil { + pm.logger.Debugf("the midpoint is safe, we should go there first") + return midJoints, nil } - panic(5) + return nil, nil } diff --git a/motionplan/constraint_checker.go b/motionplan/constraint_checker.go index 3378448f756..4ab8d2abca7 100644 --- a/motionplan/constraint_checker.go +++ b/motionplan/constraint_checker.go @@ -390,18 +390,9 @@ func InterpolateSegmentFS(ci *SegmentFS, resolution float64) ([]*referenceframe. var interpolatedConfigurations []*referenceframe.LinearInputs for i := 0; i <= maxSteps; i++ { interp := float64(i) / float64(maxSteps) - frameConfigs := referenceframe.NewLinearInputs() - - // Interpolate each frame's configuration - for frameName, startConfig := range ci.StartConfiguration.Items() { - endConfig := ci.EndConfiguration.Get(frameName) - frame := ci.FS.Frame(frameName) - - interpConfig, err := frame.Interpolate(startConfig, endConfig, interp) - if err != nil { - return nil, err - } - frameConfigs.Put(frameName, interpConfig) + frameConfigs, err := InterpLinear(ci.FS, ci.StartConfiguration, ci.EndConfiguration, interp) + if err != nil { + return nil, err } interpolatedConfigurations = append(interpolatedConfigurations, frameConfigs) @@ -410,6 +401,25 @@ func InterpolateSegmentFS(ci *SegmentFS, resolution float64) ([]*referenceframe. return interpolatedConfigurations, nil } +// InterpLinear interpolates between 2 LinearInputs by interp +func InterpLinear(fs *referenceframe.FrameSystem, + start, end *referenceframe.LinearInputs, interp float64, +) (*referenceframe.LinearInputs, error) { + mid := referenceframe.NewLinearInputs() + + for frameName, startConfig := range start.Items() { + endConfig := end.Get(frameName) + frame := fs.Frame(frameName) + + interpConfig, err := frame.Interpolate(startConfig, endConfig, interp) + if err != nil { + return nil, err + } + mid.Put(frameName, interpConfig) + } + return mid, nil +} + // CheckSegmentAndStateValidity will check an segment input and confirm that it 1) meets all segment constraints, and 2) meets all // state constraints across the segment at some resolution. If it fails an intermediate state, it will return the shortest valid segment, // provided that segment also meets segment constraints.