RSDK-8835 motionplan should support planning both from and to either a pose or a list of possible configurations

components/base/kinematicbase/ptgKinematics_test.go

@@ -77,13 +77,14 @@ func TestPTGKinematicsNoGeom(t *testing.T) {
 	fs.AddFrame(f, fs.World())
 	inputMap := referenceframe.StartPositions(fs)
 
+	startState := motionplan.NewPlanState(motionplan.PathState{f.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)}, inputMap)
+	goalState := motionplan.NewPlanState(motionplan.PathState{f.Name(): dstPIF}, nil)
+
 	plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{
-		Logger:             logger,
-		Goal:               dstPIF,
-		Frame:              f,
-		StartConfiguration: inputMap,
-		StartPose:          spatialmath.NewZeroPose(),
-		FrameSystem:        fs,
+		Logger:      logger,
+		Goals:       []*motionplan.PlanState{goalState},
+		StartState:  startState,
+		FrameSystem: fs,
 	})
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, plan, test.ShouldNotBeNil)
@@ -164,14 +165,14 @@ func TestPTGKinematicsWithGeom(t *testing.T) {
 	)
 	test.That(t, err, test.ShouldBeNil)
 
+	startState := motionplan.NewPlanState(motionplan.PathState{f.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)}, inputMap)
+	goalState := motionplan.NewPlanState(motionplan.PathState{f.Name(): dstPIF}, nil)
 	plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{
-		Logger:             logger,
-		Goal:               dstPIF,
-		Frame:              f,
-		StartConfiguration: inputMap,
-		FrameSystem:        fs,
-		WorldState:         worldState,
-		StartPose:          spatialmath.NewZeroPose(),
+		Logger:      logger,
+		Goals:       []*motionplan.PlanState{goalState},
+		StartState:  startState,
+		FrameSystem: fs,
+		WorldState:  worldState,
 	})
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, plan, test.ShouldNotBeNil)

motionplan/cBiRRT.go

@@ -93,11 +93,17 @@ func newCBiRRTMotionPlanner(
 	}, nil
 }
 
-func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, goal PathStep, seed map[string][]referenceframe.Input) ([]node, error) {
-	mp.planOpts.setGoal(goal)
+func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, seed, goal *PlanState) ([]node, error) {
 	solutionChan := make(chan *rrtSolution, 1)
+	initMaps := initRRTSolutions(ctx, atomicWaypoint{mp: mp, startState: seed, goalState: goal})
+	if initMaps.err != nil {
+		return nil, initMaps.err
+	}
+	if initMaps.steps != nil {
+		return initMaps.steps, nil
+	}
 	utils.PanicCapturingGo(func() {
-		mp.rrtBackgroundRunner(ctx, seed, &rrtParallelPlannerShared{nil, nil, solutionChan})
+		mp.rrtBackgroundRunner(ctx, &rrtParallelPlannerShared{initMaps.maps, nil, solutionChan})
 	})
 	solution := <-solutionChan
 	if solution.err != nil {
@@ -110,10 +116,10 @@ func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, goal PathStep, seed map
 // 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 map[string][]referenceframe.Input,
 	rrt *rrtParallelPlannerShared,
 ) {
 	defer close(rrt.solutionChan)
+	mp.logger.CDebugf(ctx, "starting cbirrt with start map len %d and goal map len %d\n", len(rrt.maps.startMap), len(rrt.maps.goalMap))
 
 	// setup planner options
 	if mp.planOpts == nil {
@@ -128,13 +134,13 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
 	defer cancel()
 	mp.start = time.Now()
 
-	if rrt.maps == nil || len(rrt.maps.goalMap) == 0 {
-		planSeed := initRRTSolutions(ctx, mp, seed)
-		if planSeed.err != nil || planSeed.steps != nil {
-			rrt.solutionChan <- planSeed
-			return
+	var seed map[string][]referenceframe.Input
+	// Pick a random (first in map) seed node to create the first interp node
+	for sNode, parent := range rrt.maps.startMap {
+		if parent == nil {
+			seed = sNode.Q()
+			break
 		}
-		rrt.maps = planSeed.maps
 	}
 	mp.logger.CInfof(ctx, "goal node: %v\n", rrt.maps.optNode.Q())
 	interpConfig, err := referenceframe.InterpolateFS(mp.fs, seed, rrt.maps.optNode.Q(), 0.5)

motionplan/cBiRRT_test.go

@@ -39,14 +39,13 @@ func TestSimpleLinearMotion(t *testing.T) {
 	goalPos := spatialmath.NewPose(r3.Vector{X: 206, Y: 100, Z: 120.5}, &spatialmath.OrientationVectorDegrees{OY: -1})
 
 	opt := newBasicPlannerOptions()
-	opt.setGoal(PathStep{m.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos)})
+	goalMetric := opt.getGoalMetric(PathState{m.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos)})
 	fs := referenceframe.NewEmptyFrameSystem("")
 	fs.AddFrame(m, fs.World())
 	mp, err := newCBiRRTMotionPlanner(fs, rand.New(rand.NewSource(42)), logger, opt)
 	test.That(t, err, test.ShouldBeNil)
 	cbirrt, _ := mp.(*cBiRRTMotionPlanner)
-
-	solutions, err := mp.getSolutions(ctx, map[string][]referenceframe.Input{m.Name(): home7})
+	solutions, err := mp.getSolutions(ctx, map[string][]referenceframe.Input{m.Name(): home7}, goalMetric)
 	test.That(t, err, test.ShouldBeNil)
 
 	near1 := &basicNode{q: map[string][]referenceframe.Input{m.Name(): home7}}

motionplan/check.go

@@ -45,9 +45,24 @@ func CheckPlan(
 	if err != nil {
 		return err
 	}
+
+	// Spot check plan for options
+	planOpts, err := sfPlanner.plannerSetupFromMoveRequest(
+		&PlanState{poses: plan.Path()[0]},
+		&PlanState{poses: plan.Path()[len(plan.Path())-1]},
+		plan.Trajectory()[0],
+		worldState,
+		nil,
+		nil, // no pb.Constraints
+		nil, // no plannOpts
+	)
+	if err != nil {
+		return err
+	}
+
 	// This should be done for any plan whose configurations are specified in relative terms rather than absolute ones.
 	// Currently this is only TP-space, so we check if the PTG length is >0.
-	if sfPlanner.useTPspace {
+	if planOpts.useTPspace {
 		return checkPlanRelative(checkFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner)
 	}
 	return checkPlanAbsolute(checkFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner)
@@ -80,8 +95,8 @@ func checkPlanRelative(
 
 	// setup the planOpts. Poses should be in world frame. This allows us to know e.g. which obstacles may ephemerally collide.
 	if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest(
-		plan.Path()[0],
-		plan.Path()[len(plan.Path())-1],
+		&PlanState{poses: plan.Path()[0], configuration: plan.Trajectory()[0]},
+		&PlanState{poses: plan.Path()[len(plan.Path())-1]},
 		plan.Trajectory()[0],
 		worldState,
 		nil,
@@ -217,13 +232,13 @@ func checkPlanAbsolute(
 
 	// setup the planOpts
 	if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest(
-		executionState.CurrentPoses(),
-		poses[len(poses)-1],
+		&PlanState{poses: executionState.CurrentPoses(), configuration: startingInputs},
+		&PlanState{poses: poses[len(poses)-1]},
 		startingInputs,
 		worldState,
 		nil,
-		nil, // no pb.Constraints
-		nil, // no plannOpts
+		nil, // no Constraints
+		nil, // no planOpts
 	); err != nil {
 		return err
 	}

motionplan/constraint.go

@@ -709,7 +709,7 @@ func (c *Constraints) GetCollisionSpecification() []CollisionSpecification {
 type fsPathConstraint struct {
 	metricMap     map[string]ik.StateMetric
 	constraintMap map[string]StateConstraint
-	goalMap       PathStep
+	goalMap       PathState
 	fs            referenceframe.FrameSystem
 }
 
@@ -755,7 +755,7 @@ func (fpc *fsPathConstraint) metric(state *ik.StateFS) float64 {
 func newFsPathConstraintSeparatedLinOrientTol(
 	fs referenceframe.FrameSystem,
 	startCfg map[string][]referenceframe.Input,
-	from, to PathStep,
+	from, to PathState,
 	constructor func(spatial.Pose, spatial.Pose, float64, float64) (StateConstraint, ik.StateMetric),
 	linTol, orientTol float64,
 ) (*fsPathConstraint, error) {
@@ -787,7 +787,7 @@ func newFsPathConstraintSeparatedLinOrientTol(
 func newFsPathConstraintTol(
 	fs referenceframe.FrameSystem,
 	startCfg map[string][]referenceframe.Input,
-	from, to PathStep,
+	from, to PathState,
 	constructor func(spatial.Pose, spatial.Pose, float64) (StateConstraint, ik.StateMetric),
 	tolerance float64,
 ) (*fsPathConstraint, error) {
@@ -822,7 +822,7 @@ func newFsPathConstraintTol(
 func CreateSlerpOrientationConstraintFS(
 	fs referenceframe.FrameSystem,
 	startCfg map[string][]referenceframe.Input,
-	from, to PathStep,
+	from, to PathState,
 	tolerance float64,
 ) (StateFSConstraint, ik.StateFSMetric, error) {
 	constraintInternal, err := newFsPathConstraintTol(fs, startCfg, from, to, NewSlerpOrientationConstraint, tolerance)
@@ -838,7 +838,7 @@ func CreateSlerpOrientationConstraintFS(
 func CreateLineConstraintFS(
 	fs referenceframe.FrameSystem,
 	startCfg map[string][]referenceframe.Input,
-	from, to PathStep,
+	from, to PathState,
 	tolerance float64,
 ) (StateFSConstraint, ik.StateFSMetric, error) {
 	// Need to define a constructor here since NewLineConstraint takes r3.Vectors, not poses
@@ -858,7 +858,7 @@ func CreateLineConstraintFS(
 func CreateAbsoluteLinearInterpolatingConstraintFS(
 	fs referenceframe.FrameSystem,
 	startCfg map[string][]referenceframe.Input,
-	from, to PathStep,
+	from, to PathState,
 	linTol, orientTol float64,
 ) (StateFSConstraint, ik.StateFSMetric, error) {
 	constraintInternal, err := newFsPathConstraintSeparatedLinOrientTol(
@@ -882,7 +882,7 @@ func CreateAbsoluteLinearInterpolatingConstraintFS(
 func CreateProportionalLinearInterpolatingConstraintFS(
 	fs referenceframe.FrameSystem,
 	startCfg map[string][]referenceframe.Input,
-	from, to PathStep,
+	from, to PathState,
 	linTol, orientTol float64,
 ) (StateFSConstraint, ik.StateFSMetric, error) {
 	constraintInternal, err := newFsPathConstraintSeparatedLinOrientTol(

motionplan/constraint_handler.go

@@ -113,7 +113,7 @@ func interpolateSegment(ci *ik.Segment, resolution float64) ([][]referenceframe.
 		return nil, err
 	}
 
-	steps := PathStepCount(ci.StartPosition, ci.EndPosition, resolution)
+	steps := PathStateCount(ci.StartPosition, ci.EndPosition, resolution)
 	if steps < defaultMinStepCount {
 		// Minimum step count ensures we are not missing anything
 		steps = defaultMinStepCount
@@ -163,7 +163,7 @@ func interpolateSegmentFS(ci *ik.SegmentFS, resolution float64) ([]map[string][]
 		}
 
 		// Calculate steps needed for this frame
-		steps := PathStepCount(startPos, endPos, resolution)
+		steps := PathStateCount(startPos, endPos, resolution)
 		if steps > maxSteps {
 			maxSteps = steps
 		}

motionplan/constraint_test.go

@@ -29,15 +29,16 @@ func TestIKTolerances(t *testing.T) {
 	test.That(t, err, test.ShouldBeNil)
 
 	// Test inability to arrive at another position due to orientation
-	pos := PathStep{m.Name(): frame.NewPoseInFrame(frame.World, spatial.NewPoseFromProtobuf(&commonpb.Pose{
+	goal := &PlanState{poses: PathState{m.Name(): frame.NewPoseInFrame(frame.World, spatial.NewPoseFromProtobuf(&commonpb.Pose{
 		X:  -46,
 		Y:  0,
 		Z:  372,
 		OX: -1.78,
 		OY: -3.3,
 		OZ: -1.11,
-	}))}
-	_, err = mp.plan(context.Background(), pos, map[string][]frame.Input{m.Name(): frame.FloatsToInputs(make([]float64, 6))})
+	}))}}
+	seed := &PlanState{configuration: map[string][]frame.Input{m.Name(): frame.FloatsToInputs(make([]float64, 6))}}
+	_, err = mp.plan(context.Background(), seed, goal)
 	test.That(t, err, test.ShouldNotBeNil)
 
 	// Now verify that setting tolerances to zero allows the same arm to reach that position
@@ -46,7 +47,7 @@ func TestIKTolerances(t *testing.T) {
 	opt.SetMaxSolutions(50)
 	mp, err = newCBiRRTMotionPlanner(fs, rand.New(rand.NewSource(1)), logger, opt)
 	test.That(t, err, test.ShouldBeNil)
-	_, err = mp.plan(context.Background(), pos, map[string][]frame.Input{m.Name(): frame.FloatsToInputs(make([]float64, 6))})
+	_, err = mp.plan(context.Background(), seed, goal)
 	test.That(t, err, test.ShouldBeNil)
 }
 
@@ -150,8 +151,8 @@ func TestLineFollow(t *testing.T) {
 
 	opt := newBasicPlannerOptions()
 	startCfg := map[string][]frame.Input{m.Name(): m.InputFromProtobuf(mp1)}
-	from := PathStep{markerFrame.Name(): frame.NewPoseInFrame(markerFrame.Name(), p1)}
-	to := PathStep{markerFrame.Name(): frame.NewPoseInFrame(goalFrame.Name(), p2)}
+	from := PathState{markerFrame.Name(): frame.NewPoseInFrame(markerFrame.Name(), p1)}
+	to := PathState{markerFrame.Name(): frame.NewPoseInFrame(goalFrame.Name(), p2)}
 
 	validFunc, gradFunc, err := CreateLineConstraintFS(fs, startCfg, from, to, 0.001)
 	test.That(t, err, test.ShouldBeNil)

motionplan/ik/metrics.go

@@ -264,6 +264,8 @@ func FSConfigurationDistance(segment *SegmentFS) float64 {
 			for i, val := range cfg {
 				score += math.Abs(val.Value - endCfg[i].Value)
 			}
+		} else {
+			score += math.Inf(1)
 		}
 	}
 	return score
@@ -275,6 +277,8 @@ func FSConfigurationL2Distance(segment *SegmentFS) float64 {
 	for frame, cfg := range segment.StartConfiguration {
 		if endCfg, ok := segment.EndConfiguration[frame]; ok && len(cfg) == len(endCfg) {
 			score += referenceframe.InputsL2Distance(cfg, endCfg)
+		} else {
+			score += math.Inf(1)
 		}
 	}
 	return score