Add pose state machine v2 by danielbotros · Pull Request #37 · viamrobotics/beanjamin

danielbotros · 2026-03-09T17:01:44Z

No description provided.

Introduces a state machine that enforces valid robot pose transitions throughout the coffee-making workflow, preventing unsafe position jumps. State layout (indices 0–10): home → grinder_approach → grinder_activate → grinder_approach → tamper_approach → tamper_activate → tamper_approach → coffee_approach → coffee_in → coffee_locked_mid → coffee_locked_final Rules: - Approach/home states freely interconnect (any approach → any approach/home) - Each approach state can enter its own section's activate/in step - Activate and locked states can only exit to their section's own approach - coffee_locked_final must retrace step-by-step back to coffee_approach Automatic path interpolation: when a target pose is not directly reachable from the current state, BFS finds the shortest valid path and executes intermediate moves transparently before the requested step. State is auto-initialized on startup by querying the switch's current position. Manual override via set_state_index / get_state DoCommands. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

dump_grounds shares physical space with home and is used at the end of the unbrew sequence. Without this, run{"unbrew"} would fail with "no valid path to pose dump_grounds". Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

Provides a single-pose move that goes through the state machine, allowing Stream Deck debug buttons to be pointed at coffee-lifecycle instead of the switch directly. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

App flow (executeStep) now just moves and tracks state without enforcing transitions — the sequences are already programmed in the right order. move_to is the only entry point that runs BFS path finding and enforces valid transitions, making it safe for manual/debug use. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

danielbotros · 2026-03-09T17:05:34Z

 func (s *multiPosesExecutionSwitch) GetPosition(ctx context.Context, extra map[string]interface{}) (uint32, error) {
-	s.mu.Lock()
-	defer s.mu.Unlock()
-	return s.position, nil
+	if s.stateMachine == nil {
+		return 0, errors.New("state machine not configured; cannot determine current position")
+	}
+	resp, err := s.stateMachine.DoCommand(ctx, map[string]interface{}{"get_state": true})
+	if err != nil {
+		return 0, fmt.Errorf("failed to get state machine state: %w", err)
+	}
+	stateIdx, ok := resp["state_index"].(int)
+	if !ok {
+		return 0, errors.New("unexpected state_index type from state machine")
+	}
+	if stateIdx < 0 {
+		return 0, errors.New("state machine is uninitialized; use set_state_index to initialize")
+	}
+	currentPoseName := statemachine.PoseNameAt(stateIdx)
+	for i, name := range s.poseNames {
+		if name == currentPoseName {
+			return uint32(i), nil
+		}
+	}
+	return 0, fmt.Errorf("state machine current pose %q is not in this switch's pose list", currentPoseName)


State machine will now be the ground truth of our position. Tracks across run sequences and prepare_order calls as well.

…mid from state machine

danielbotros · 2026-03-09T18:43:39Z

-		select {
-		case <-ctx.Done():
-			return nil, fmt.Errorf("%s cancelled at step %d (%q): %w", label, i, step.PoseName, ctx.Err())
-		case <-cancelCtx.Done():
-			return nil, fmt.Errorf("%s cancelled at step %d (%q)", label, i, step.PoseName)
-		default:
-		}
-
-		s.logger.Infof("%s step %d/%d: moving to %q", label, i+1, len(steps), step.PoseName)
-
-		if err := s.moveToPose(ctx, step); err != nil {
-			return nil, fmt.Errorf("%s failed at step %d (%q): %w", label, i, step.PoseName, err)
-		}
-
-		if step.PauseSec > 0 {
-			pause := time.Duration(step.PauseSec * float64(time.Second))
-			s.logger.Infof("pausing %s after %q", pause, step.PoseName)
-			select {
-			case <-time.After(pause):
-			case <-ctx.Done():
-				return nil, fmt.Errorf("%s cancelled during pause after %q: %w", label, step.PoseName, ctx.Err())
-			case <-cancelCtx.Done():
-				return nil, fmt.Errorf("%s cancelled during pause after %q", label, step.PoseName)
-			}


executeStep gives us cancellation and pauses already; this is just clean up

danielbotros · 2026-03-09T18:47:31Z

@@ -228,6 +268,14 @@ func (s *beanjaminCoffee) DoCommand(ctx context.Context, cmd map[string]interfac
 		for i := len(steps) - 2; i >= 0; i-- {
 			reversed = append(reversed, steps[i])
 		}
+		reversedPoseNames := make([]string, len(reversed))
+		for i, step := range reversed {
+			reversedPoseNames[i] = step.PoseName
+		}
+		if err := statemachine.ValidatePath(reversedPoseNames, statemachine.InferIndex(steps[len(steps)-1].PoseName)); err != nil {
+			s.logger.Errorw("DoCommand", "error", err)
+			return nil, err
+		}


Validates sequence against state machine to catch any user errors

danielbotros · 2026-03-09T21:33:55Z

+// detectCurrentPose queries the component's current pose via the motion service and
+// finds the closest configured pose within tolerance. Returns the pose name, or an
+// error if no pose matches.
+func (s *multiPosesExecutionSwitch) detectCurrentPose(ctx context.Context) (string, error) {


State machine needs an initial state. It will try to automatically detect it by seeing if its current position is within a 5mm/degree tolerance of any known poses. If not, a set_state DoCommand must be run to set it. Only must be done once on start up. If we run a prepare order command or a run sequence, that will also initialize a state.

danielbotros and others added 6 commits March 9, 2026 10:33

Add move_to DoCommand for safe debug page control

2817ab6

Provides a single-pose move that goes through the state machine, allowing Stream Deck debug buttons to be pointed at coffee-lifecycle instead of the switch directly. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

Replace interface{} with any

74e4399

Add state machine

5845862

danielbotros commented Mar 9, 2026

View reviewed changes

danielbotros added 3 commits March 9, 2026 13:17

any --> interface, make state machine required, remove coffee locked …

a4e8565

…mid from state machine

Remove new move_to

63d7266

Make switch cancellable, logging fixes, graph fixes and clean up

3839398

danielbotros commented Mar 9, 2026

View reviewed changes

danielbotros added 7 commits March 9, 2026 14:51

Do not allow any unknown poses

a7a062c

Make graph configurable

d29b186

Clean up and fix closing race

d7347ef

Automatic position detection

21e1cd6

Filter, not arm

aeaa476

Refactor to make state machine internal

63597e7

Re-add set state

1de0b0e

danielbotros commented Mar 9, 2026

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add pose state machine v2#37

Add pose state machine v2#37
danielbotros wants to merge 16 commits into
mainfrom
add-pose-state-machine-v2

danielbotros commented Mar 9, 2026

Uh oh!

danielbotros Mar 9, 2026 •

edited

Loading

Uh oh!

danielbotros Mar 9, 2026 •

edited

Loading

Uh oh!

danielbotros Mar 9, 2026

Uh oh!

danielbotros Mar 9, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

Conversation

danielbotros commented Mar 9, 2026

Uh oh!

danielbotros Mar 9, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

danielbotros Mar 9, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

danielbotros Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

danielbotros Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

danielbotros Mar 9, 2026 •

edited

Loading

danielbotros Mar 9, 2026 •

edited

Loading