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

biotinker · 2024-12-12T00:27:57Z

This PR is not quite complete; there are not yet tests for the new behavior. Tests will be added before merge, and bugs turned up by the tests may merit minor changes to the body of code presented here.

This PR is a breaking change to motionplan. It introduces a new struct, PlanState, which may be a PathStep, a configuration, or both. PlanRequest undergoes the breaking change, removing several public fields and replacing them with new fields which must be PlanStates.

The changes here enable an arbitrary number of waypoints to be specified, and the planner will plan to each one in turn. It will do so faster than it would if each one were an individual Plan call.

Additionally, it permits two new interesting pieces of behavior:

The starting state of the robot may be specified as a set of poses, rather than a configuration (current or otherwise). If so, IK will be used to find a starting configuration.
The goal state of the robot may be specified as a configuration, rather than a set of poses. This allows to plan to a known-good state, such as from a prior executed plan, or to connect to a plan created by specifying a start pose as mentioned above.

… goals, which can be configurations or sets of poses.

…-planning-both-from-and-to-either-a-pose-or-a-list-of-possible-configurations

biotinker · 2024-12-12T00:28:44Z

motionplan/motionPlanner.go

 	}

-	// TODO: This should replace Frame and Goal
-	req.allGoals = map[string]*referenceframe.PoseInFrame{req.Frame.Name(): req.Goal}
+	for i, goalState := range req.Goals {


TODO: comment this better

biotinker · 2024-12-12T00:30:02Z

motionplan/node.go

+	"go.viam.com/rdk/referenceframe"
+)
+
+// fixedStepInterpolation returns inputs at qstep distance along the path from start to target


These functions are not new, they are merely moved over from rrt.go.

motionplan/node.go

biotinker · 2024-12-12T00:30:51Z

motionplan/planManager.go

 	maps *rrtMaps,
 ) (map[string][]referenceframe.Input, *resultPromise, error) {
-	if parPlan, ok := pathPlanner.(rrtParallelPlanner); ok {
+	// ~ pm.logger.Debug("start planning for ", goal.ToProto())


biotinker · 2024-12-12T00:31:10Z

motionplan/planManager.go

+		if err != nil {
+			return nil, err
+		}
+		// ~ wpOpt.setGoal(to)


biotinker · 2024-12-12T00:31:39Z

motionplan/plannerOptions.go

@@ -124,7 +123,6 @@ type plannerOptions struct {
 	ConstraintHandler
 	motionChains          []*motionChain
 	goalMetricConstructor func(spatialmath.Pose) ik.StateMetric


Comment use of this

biotinker · 2024-12-12T00:32:27Z

robot/client/client_test.go

@@ -984,7 +984,7 @@ func TestClientStreamDisconnectHandler(t *testing.T) {
 		t.Helper()

 		client.connected.Store(false)
-		//nolint:staticcheck // the status API is deprecated


These shouldn't have changed I think...darn linter

raybjork

Overall I like the shape of it. I think we should think a little more about how we want to call these structs which will become the building blocks of this package. If we come up with something coherent I think it will make the mental load of understanding this package lighter to new developers

motionplan/node.go

raybjork · 2024-12-12T16:51:43Z

motionplan/plan.go

+
+// PlanState is a struct which holds both a PathStep and a configuration. This is intended to be used as start or goal states for plans.
+// Either field may be nil.
+type PlanState struct {


I find the way the names for these objects slightly confusing. Maybe rename to PlanStep? Or if not rename PathStep to PathState? I think I like the latter better. That way we have a Plan & PlanState, Path & PathState, in the future Trajectory & TrajectoryState. I'm open to other ideas on what to actually call them but I think we should try to draw parallels here in our nomenclature, otherwise some of these names are a bit confusing.

raybjork · 2024-12-12T16:58:05Z

motionplan/cBiRRT.go

 	utils.PanicCapturingGo(func() {
-		mp.rrtBackgroundRunner(ctx, seed, &rrtParallelPlannerShared{nil, nil, solutionChan})
+		mp.rrtBackgroundRunner(ctx, &rrtParallelPlannerShared{
+			initMaps.maps,


[style] this doesnt have to be broken up to multiple lines. Or if you want to do this can you add the field names to the struct instantiation?

Good point; I think I was originally prototyping this with a much longer line, and had to split to read it, then shrunk it later

raybjork · 2024-12-12T17:02:07Z

motionplan/cBiRRT.go

+	// 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()


pretty sure you want to be breaking out of this loop here

raybjork · 2024-12-12T17:05:15Z

motionplan/check.go

-		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


as long as youre fixing these comments this should read Constraints now

raybjork · 2024-12-12T21:39:56Z

motionplan/plan.go

+}
+
+// DeserializePlanState turns a serialized PlanState back into a PlanState.
+func DeserializePlanState(iface map[string]interface{}) (*PlanState, error) {


Do you have round trip tests for this?

raybjork · 2024-12-12T21:42:20Z

motionplan/planManager.go

-			referenceframe.PoseInFrameToProtobuf(startPose.(*referenceframe.PoseInFrame)),
-			request.WorldState.String(),
-		)
-	}
 	opt, err := pm.plannerSetupFromMoveRequest(


The arguments of this function can be cleaned up a little I think. We're passing redundant arguments

raybjork · 2024-12-12T21:46:12Z

motionplan/planManager.go

-			return nil, err
+
+		// Log each requested motion
+		for frame, goal := range goalPoses {


Could be worth it to make a String function on PlanRequest

raybjork · 2024-12-12T21:49:18Z

motionplan/planManager.go

@@ -758,6 +670,122 @@ func (pm *planManager) plannerSetupFromMoveRequest(
 	return opt, nil
 }

+// generateWaypoints will return the list of atomic waypoints that correspond to a specific goal in a plan request.
+func (pm *planManager) generateWaypoints(request *PlanRequest, seedPlan Plan, wpi int) ([]atomicWaypoint, error) {


It looks like a lot of this was moved from above should I do a deeper read of this?

motionplan/planManager.go

…, more to do on creating motion chains when goal is in frame of itself.

biotinker added 4 commits December 11, 2024 09:29

Complete, test-passing update to motionplan to take a set of waypoint…

1f4b9f0

… goals, which can be configurations or sets of poses.

Updates to motion service to enable use of waypoints via extra

48bd327

Merge branch 'main' into 20241204_RSDK-8835-motionplan-should-support…

0bad1af

…-planning-both-from-and-to-either-a-pose-or-a-list-of-possible-configurations

Appease linter

bfa17a1

biotinker requested review from raybjork and nfranczak December 12, 2024 00:28

viambot added the safe to test This pull request is marked safe to test from a trusted zone label Dec 12, 2024

biotinker commented Dec 12, 2024

View reviewed changes

motionplan/node.go Show resolved Hide resolved

biotinker commented Dec 12, 2024

View reviewed changes

motionplan/planManager.go Outdated

if err != nil {

return nil, err

}

// ~ wpOpt.setGoal(to)

Copy link

Member Author

biotinker Dec 12, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Delete

biotinker commented Dec 12, 2024

View reviewed changes

raybjork requested changes Dec 12, 2024

View reviewed changes

Assorted work on adding tests and fixing the bugs they turned up. WIP…

d9182c1

…, more to do on creating motion chains when goal is in frame of itself.

viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Dec 12, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

biotinker commented Dec 12, 2024

biotinker Dec 12, 2024

biotinker Dec 12, 2024

biotinker Dec 12, 2024

biotinker Dec 12, 2024

biotinker Dec 12, 2024

biotinker Dec 12, 2024

raybjork left a comment

raybjork Dec 12, 2024

raybjork Dec 12, 2024

biotinker Dec 12, 2024

raybjork Dec 12, 2024

biotinker Dec 12, 2024

raybjork Dec 12, 2024

raybjork Dec 12, 2024

biotinker Dec 12, 2024

raybjork Dec 12, 2024

raybjork Dec 12, 2024

raybjork Dec 12, 2024

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

Are you sure you want to change the base?

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

Conversation

biotinker commented Dec 12, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

raybjork left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment