Add spatialmath worksheet game for learning spatial transformations

spatialmath/worksheet/cmd/main.go

@@ -0,0 +1,33 @@
+// Package main provides an interactive CLI game for learning spatialmath transformations.
+//
+// Run with: go run ./spatialmath/worksheet/cmd
+// Jump to a level: go run ./spatialmath/worksheet/cmd --level 3
+package main
+
+import (
+	"bufio"
+	"flag"
+	"fmt"
+	"os"
+
+	"go.viam.com/rdk/spatialmath/worksheet"
+)
+
+func main() {
+	levelFlag := flag.Int("level", 0, "Jump to a specific level (1-5). 0 runs all levels.")
+	flag.Parse()
+
+	levels := worksheet.MakeLevels()
+	reader := bufio.NewReader(os.Stdin)
+
+	if *levelFlag < 0 || *levelFlag > len(levels) {
+		fmt.Fprintf(os.Stderr, "Invalid level %d. Choose 1-%d, or 0 for all.\n", *levelFlag, len(levels))
+		os.Exit(1)
+	}
+
+	if *levelFlag > 0 {
+		worksheet.RunLevel(reader, levels[*levelFlag-1], len(levels))
+	} else {
+		worksheet.RunAllLevels(reader, levels)
+	}
+}

spatialmath/worksheet/format.go

@@ -0,0 +1,37 @@
+package worksheet
+
+import (
+	"fmt"
+	"math"
+
+	"github.com/golang/geo/r3"
+
+	"go.viam.com/rdk/spatialmath"
+)
+
+// snapFloat rounds values very close to integers to that integer, for clean display.
+func snapFloat(f float64) float64 {
+	rounded := math.Round(f)
+	if math.Abs(f-rounded) < 1e-6 {
+		return rounded
+	}
+	return math.Round(f*100) / 100
+}
+
+// FormatPoint formats an r3.Vector for display.
+func FormatPoint(v r3.Vector) string {
+	return fmt.Sprintf("r3.Vector{X: %g, Y: %g, Z: %g}", snapFloat(v.X), snapFloat(v.Y), snapFloat(v.Z))
+}
+
+// FormatOrientation formats an Orientation as OrientationVectorDegrees for display.
+func FormatOrientation(o spatialmath.Orientation) string {
+	ovd := o.OrientationVectorDegrees()
+	return fmt.Sprintf("OrientationVectorDegrees{Theta: %g, OX: %g, OY: %g, OZ: %g}",
+		snapFloat(ovd.Theta), snapFloat(ovd.OX), snapFloat(ovd.OY), snapFloat(ovd.OZ))
+}
+
+// FormatPose formats a Pose showing both point and orientation.
+func FormatPose(p spatialmath.Pose) string {
+	return fmt.Sprintf("Point:       %s\n    Orientation: %s",
+		FormatPoint(p.Point()), FormatOrientation(p.Orientation()))
+}

spatialmath/worksheet/game.go

@@ -0,0 +1,153 @@
+// Package worksheet provides an interactive CLI game for learning spatialmath transformations.
+package worksheet
+
+import (
+	"bufio"
+	"fmt"
+	"sort"
+	"strings"
+
+	"go.viam.com/rdk/spatialmath"
+)
+
+// Question represents a single exercise in the worksheet.
+type Question struct {
+	// Setup is the Go code snippet shown to the user.
+	Setup string
+	// Answer is the formatted result string.
+	Answer string
+	// Explanation describes why the answer is what it is.
+	Explanation string
+	// InputPoses are the named poses to visualize before the answer is revealed.
+	// Keys should match variable names in Setup (e.g. "a", "b").
+	InputPoses map[string]spatialmath.Pose
+	// ResultPose is the result to visualize after the answer is revealed.
+	ResultPose spatialmath.Pose
+}
+
+// Level represents a cohesive set of exercises.
+type Level struct {
+	Number      int
+	Title       string
+	Description string
+	Questions   []Question
+}
+
+// print helpers — this is a CLI tool, stdout output is intentional.
+
+//nolint:forbidigo
+func printLine(a ...any) { fmt.Println(a...) }
+
+//nolint:forbidigo
+func printFmt(format string, a ...any) { fmt.Printf(format, a...) }
+
+//nolint:forbidigo
+func printPrompt(s string) { fmt.Print(s) }
+
+func waitForEnter(reader *bufio.Reader) {
+	//nolint:errcheck
+	reader.ReadString('\n')
+}
+
+// inputPoseLegend builds a human-readable legend mapping colors to variables.
+func inputPoseLegend(poses map[string]spatialmath.Pose) string {
+	if len(poses) == 0 {
+		return ""
+	}
+	names := make([]string, 0, len(poses))
+	for name := range poses {
+		names = append(names, name)
+	}
+	sort.Strings(names)
+
+	parts := make([]string, 0, len(names)+1)
+	parts = append(parts, "white = origin")
+	for i, name := range names {
+		color := PoseColorByIndex(i)
+		parts = append(parts, color+" = "+name)
+	}
+	return strings.Join(parts, ", ")
+}
+
+// RunLevel runs a single level interactively.
+func RunLevel(reader *bufio.Reader, level Level, totalLevels int) {
+	printFmt("\n=== Level %d: %s (%d/%d) ===\n",
+		level.Number, level.Title, level.Number, totalLevels)
+	printLine(level.Description)
+	printLine()
+
+	for i, q := range level.Questions {
+		printFmt("--- Question %d of %d ---\n\n",
+			i+1, len(level.Questions))
+
+		if len(q.InputPoses) > 0 {
+			DrawInputPoses(q.InputPoses)
+			legend := inputPoseLegend(q.InputPoses)
+			printFmt("  3D view: %s\n", legend)
+			printLine("  Each pose is a 10x20x30 box so you can see orientation.")
+			printLine()
+		}
+
+		printLine(q.Setup)
+		printLine()
+		printLine("What is the result?")
+		printLine()
+		printPrompt("Press Enter when you've thought about it...")
+		waitForEnter(reader)
+
+		printLine()
+		printFmt("  Answer:\n    %s\n", q.Answer)
+
+		if q.ResultPose != nil {
+			DrawResult(q.ResultPose)
+			printLine()
+			printLine("  3D view: red = result (added to the scene)")
+		}
+
+		if q.Explanation != "" {
+			printLine()
+			printFmt("  %s\n", q.Explanation)
+		}
+
+		printLine()
+		if i < len(level.Questions)-1 {
+			printPrompt("Press Enter for next question...")
+		} else {
+			printPrompt("Press Enter to finish this level...")
+		}
+		waitForEnter(reader)
+		printLine()
+	}
+
+	ClearVisualization()
+	printFmt("=== Level %d Complete! ===\n\n", level.Number)
+}
+
+// RunAllLevels runs all levels sequentially.
+func RunAllLevels(reader *bufio.Reader, levels []Level) {
+	printLine("=== Spatialmath Worksheet Game ===")
+	printFmt("Learn spatial transformations through %d levels.\n",
+		len(levels))
+	printLine()
+	printLine("3D Visualization (requires motion-tools running):")
+	printLine("  - Poses drawn as 10x20x30 mm boxes (asymmetric)")
+	printLine("  - white  = origin (reference)")
+	printLine("  - blue   = 1st input pose")
+	printLine("  - green  = 2nd input pose")
+	printLine("  - yellow = 3rd input pose")
+	printLine("  - red    = result (after reveal)")
+	printLine("  Variable names shown in each question's legend.")
+	printLine()
+	printPrompt("Press Enter to begin...")
+	waitForEnter(reader)
+
+	for _, level := range levels {
+		RunLevel(reader, level, len(levels))
+		if level.Number < len(levels) {
+			printPrompt("Press Enter to continue to next level...")
+			waitForEnter(reader)
+		}
+	}
+
+	printLine("=== All levels complete! ===")
+}