Skip to content

try going halfway on cbirrt before going down full path #5457

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

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

try going halfway on cbirrt before going down full path #5457

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
86 changes: 28 additions & 58 deletions motionplan/armplanning/plan_manager.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
}
Expand Down Expand Up @@ -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
}
34 changes: 22 additions & 12 deletions motionplan/constraint_checker.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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.
Expand Down
Loading