diff --git a/spatialmath/worksheet/cmd/main.go b/spatialmath/worksheet/cmd/main.go new file mode 100644 index 00000000000..13789c367cb --- /dev/null +++ b/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) + } +} diff --git a/spatialmath/worksheet/format.go b/spatialmath/worksheet/format.go new file mode 100644 index 00000000000..9e53483e810 --- /dev/null +++ b/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())) +} diff --git a/spatialmath/worksheet/game.go b/spatialmath/worksheet/game.go new file mode 100644 index 00000000000..7ac18aa57d9 --- /dev/null +++ b/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! ===") +} diff --git a/spatialmath/worksheet/levels.go b/spatialmath/worksheet/levels.go new file mode 100644 index 00000000000..ea41e81be9f --- /dev/null +++ b/spatialmath/worksheet/levels.go @@ -0,0 +1,361 @@ +package worksheet + +import ( + "fmt" + + "github.com/golang/geo/r3" + + "go.viam.com/rdk/spatialmath" +) + +// MakeLevels builds all game levels with pre-computed answers from real spatialmath calls. +func MakeLevels() []Level { + return []Level{ + makeLevel1(), + makeLevel2(), + makeLevel3(), + makeLevel4(), + makeLevel5(), + } +} + +// ovd is a shorthand for creating an OrientationVectorDegrees. +func ovd(theta, ox, oy, oz float64) *spatialmath.OrientationVectorDegrees { + return &spatialmath.OrientationVectorDegrees{Theta: theta, OX: ox, OY: oy, OZ: oz} +} + +// Level 1: Pure Translation +func makeLevel1() Level { + a1 := spatialmath.NewPoseFromPoint(r3.Vector{X: 100, Y: 100, Z: 0}) + b1 := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 100}) + r1 := spatialmath.Compose(a1, b1) + + a2 := spatialmath.NewPoseFromPoint(r3.Vector{X: 50, Y: 0, Z: 0}) + b2 := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 50, Z: 0}) + c2 := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 50}) + r2 := spatialmath.Compose(spatialmath.Compose(a2, b2), c2) + + a3 := spatialmath.NewPoseFromPoint(r3.Vector{X: 100, Y: -200, Z: 300}) + r3inv := spatialmath.PoseInverse(a3) + + a4 := spatialmath.NewPoseFromPoint(r3.Vector{X: 42, Y: 99, Z: -7}) + r4 := spatialmath.Compose(a4, spatialmath.PoseInverse(a4)) + + return Level{ + Number: 1, + Title: "Pure Translation", + Description: "Composing point-only poses — no rotation involved.\nWhat does Compose do when there is no orientation?", + Questions: []Question{ + { + Setup: ` a := spatialmath.NewPoseFromPoint(r3.Vector{X: 100, Y: 100, Z: 0}) + b := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 100}) + result := spatialmath.Compose(a, b)`, + Answer: FormatPose(r1), + Explanation: "With no orientation on either pose, Compose adds the translation vectors.", + InputPoses: map[string]spatialmath.Pose{"a": a1, "b": b1}, + ResultPose: r1, + }, + { + Setup: ` a := spatialmath.NewPoseFromPoint(r3.Vector{X: 50, Y: 0, Z: 0}) + b := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 50, Z: 0}) + c := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 50}) + result := spatialmath.Compose(spatialmath.Compose(a, b), c)`, + Answer: FormatPose(r2), + Explanation: "Chaining three translations: each Compose adds the next vector.", + InputPoses: map[string]spatialmath.Pose{"a": a2, "b": b2, "c": c2}, + ResultPose: r2, + }, + { + Setup: ` a := spatialmath.NewPoseFromPoint(r3.Vector{X: 100, Y: -200, Z: 300}) + result := spatialmath.PoseInverse(a)`, + Answer: FormatPose(r3inv), + Explanation: "PoseInverse of a pure translation negates the vector.", + InputPoses: map[string]spatialmath.Pose{"a": a3}, + ResultPose: r3inv, + }, + { + Setup: ` a := spatialmath.NewPoseFromPoint(r3.Vector{X: 42, Y: 99, Z: -7}) + result := spatialmath.Compose(a, spatialmath.PoseInverse(a))`, + Answer: FormatPose(r4), + Explanation: "Composing any pose with its inverse always returns the zero pose (identity).", + InputPoses: map[string]spatialmath.Pose{"a": a4}, + ResultPose: r4, + }, + }, + } +} + +// Level 2: Rotation from Origin +func makeLevel2() Level { + angles := []float64{0, 90, 180, 270} + point := spatialmath.NewPoseFromPoint(r3.Vector{X: 100, Y: 0, Z: 0}) + + questions := make([]Question, 0, len(angles)) + for _, deg := range angles { + rot := spatialmath.NewPose(r3.Vector{}, ovd(deg, 1, 0, 0)) + result := spatialmath.Compose(rot, point) + + setup := fmt.Sprintf(` rot := spatialmath.NewPose(r3.Vector{}, &spatialmath.OrientationVectorDegrees{Theta: %g, OX: 1, OY: 0, OZ: 0}) + point := spatialmath.NewPoseFromPoint(r3.Vector{X: 100, Y: 0, Z: 0}) + result := spatialmath.Compose(rot, point)`, deg) + + questions = append(questions, Question{ + Setup: setup, + Answer: FormatPose(result), + Explanation: fmt.Sprintf("Rotating %g° around X, then translating 100 along local X.", deg), + InputPoses: map[string]spatialmath.Pose{"rot": rot, "point": point}, + ResultPose: result, + }) + } + + return Level{ + Number: 2, + Title: "Rotation from Origin", + Description: "How orientation affects where a subsequent translation ends up.\n" + + "The first pose rotates the frame, the second translates in the rotated frame.", + Questions: questions, + } +} + +// Level 3: Compose with Orientations +func makeLevel3() Level { + // Two poses with theta rotations around OZ + a1 := spatialmath.NewPose(r3.Vector{X: 100, Y: 0, Z: 0}, ovd(45, 0, 0, 1)) + b1 := spatialmath.NewPose(r3.Vector{X: 50, Y: 0, Z: 0}, ovd(30, 0, 0, 1)) + r1 := spatialmath.Compose(a1, b1) + + // Non-commutativity: Compose(a, b) vs Compose(b, a) + r2 := spatialmath.Compose(b1, a1) + + // PoseBetween + a3 := spatialmath.NewPose(r3.Vector{X: 100, Y: 0, Z: 0}, ovd(45, 0, 0, 1)) + r3target := spatialmath.Compose(a3, b1) + r3between := spatialmath.PoseBetween(a3, r3target) + + return Level{ + Number: 3, + Title: "Compose with Orientations", + Description: "Both poses have orientations. Order matters!\nTheta rotates around the OV axis (in-line rotation).", + Questions: []Question{ + { + Setup: ` a := spatialmath.NewPose(r3.Vector{X: 100, Y: 0, Z: 0}, &spatialmath.OrientationVectorDegrees{Theta: 45, OX: 0, OY: 0, OZ: 1}) + b := spatialmath.NewPose(r3.Vector{X: 50, Y: 0, Z: 0}, &spatialmath.OrientationVectorDegrees{Theta: 30, OX: 0, OY: 0, OZ: 1}) + result := spatialmath.Compose(a, b)`, + Answer: FormatPose(r1), + Explanation: "a's 45° rotation around Z rotates b's translation. The thetas add (45+30=75°).", + InputPoses: map[string]spatialmath.Pose{"a": a1, "b": b1}, + ResultPose: r1, + }, + { + Setup: ` // Same a and b as before, but reversed! + a := spatialmath.NewPose(r3.Vector{X: 100, Y: 0, Z: 0}, &spatialmath.OrientationVectorDegrees{Theta: 45, OX: 0, OY: 0, OZ: 1}) + b := spatialmath.NewPose(r3.Vector{X: 50, Y: 0, Z: 0}, &spatialmath.OrientationVectorDegrees{Theta: 30, OX: 0, OY: 0, OZ: 1}) + result := spatialmath.Compose(b, a) // NOTE: b first, then a!`, + Answer: FormatPose(r2), + Explanation: "Compose(a,b) != Compose(b,a)! Spatial composition is NOT commutative.\n" + + " The first pose's orientation rotates the second's translation differently.", + InputPoses: map[string]spatialmath.Pose{"a": a1, "b": b1}, + ResultPose: r2, + }, + { + Setup: fmt.Sprintf( + " a := spatialmath.NewPose(\n"+ + " r3.Vector{X: 100, Y: 0, Z: 0},\n"+ + " &spatialmath.OrientationVectorDegrees{Theta: 45, OX: 0, OY: 0, OZ: 1})\n"+ + " target := // %s\n"+ + " result := spatialmath.PoseBetween(a, target)\n"+ + " // PoseBetween finds b such that Compose(a, b) = target", + FormatPoint(r3target.Point())), + Answer: FormatPose(r3between), + Explanation: "PoseBetween(a, target) recovers b" + + " -- the transform from a to target.", + InputPoses: map[string]spatialmath.Pose{ + "a": a3, "target": r3target, + }, + ResultPose: r3between, + }, + }, + } +} + +// Level 4: Full 3D Composition +func makeLevel4() Level { + // Compose with different orientation axes + a1 := spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 100}, ovd(0, 1, 0, 0)) + b1 := spatialmath.NewPose(r3.Vector{X: 50, Y: 0, Z: 0}, ovd(0, 0, 1, 0)) + r1 := spatialmath.Compose(a1, b1) + + // PoseBetween: given A and C, find B + a2 := spatialmath.NewPose(r3.Vector{X: 100, Y: 0, Z: 0}, ovd(0, 0, 0, 1)) + c2 := spatialmath.NewPose(r3.Vector{X: 100, Y: 100, Z: 0}, ovd(90, 0, 0, 1)) + r2 := spatialmath.PoseBetween(a2, c2) + + // PoseInverse of an oriented pose + a3 := spatialmath.NewPose(r3.Vector{X: 50, Y: 50, Z: 50}, ovd(45, 0, 0, 1)) + r3inv := spatialmath.PoseInverse(a3) + + // Verify: Compose(a, PoseInverse(a)) = identity + r4 := spatialmath.Compose(a3, r3inv) + + return Level{ + Number: 4, + Title: "Full 3D Composition", + Description: "Multi-axis orientations — OX, OY, OZ all in play.\nThese are the transformations that control robot arm movements.", + Questions: []Question{ + { + Setup: ` a := spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 100}, &spatialmath.OrientationVectorDegrees{Theta: 0, OX: 1, OY: 0, OZ: 0}) + b := spatialmath.NewPose(r3.Vector{X: 50, Y: 0, Z: 0}, &spatialmath.OrientationVectorDegrees{Theta: 0, OX: 0, OY: 1, OZ: 0}) + result := spatialmath.Compose(a, b)`, + Answer: FormatPose(r1), + Explanation: "a points along X (OX=1), b along Y (OY=1).\n" + + " a's orientation rotates b's translation into a different direction.", + InputPoses: map[string]spatialmath.Pose{"a": a1, "b": b1}, + ResultPose: r1, + }, + { + Setup: ` a := spatialmath.NewPose(r3.Vector{X: 100, Y: 0, Z: 0}, &spatialmath.OrientationVectorDegrees{Theta: 0, OX: 0, OY: 0, OZ: 1}) + c := spatialmath.NewPose(r3.Vector{X: 100, Y: 100, Z: 0}, &spatialmath.OrientationVectorDegrees{Theta: 90, OX: 0, OY: 0, OZ: 1}) + result := spatialmath.PoseBetween(a, c) + // What transform b makes Compose(a, b) = c?`, + Answer: FormatPose(r2), + Explanation: "PoseBetween finds the relative transform between two poses.\n" + + " Think of it as: 'what do I apply from a's frame to reach c?'", + InputPoses: map[string]spatialmath.Pose{"a": a2, "c": c2}, + ResultPose: r2, + }, + { + Setup: " a := spatialmath.NewPose(\n" + + " r3.Vector{X: 50, Y: 50, Z: 50},\n" + + " &spatialmath.OrientationVectorDegrees{Theta: 45, OX: 0, OY: 0, OZ: 1})\n" + + " result := spatialmath.PoseInverse(a)", + Answer: FormatPose(r3inv), + Explanation: "PoseInverse of an oriented pose is NOT just negating the point!\n" + + " The inverse orientation also rotates the negated translation.", + InputPoses: map[string]spatialmath.Pose{"a": a3}, + ResultPose: r3inv, + }, + { + Setup: " a := spatialmath.NewPose(\n" + + " r3.Vector{X: 50, Y: 50, Z: 50},\n" + + " &spatialmath.OrientationVectorDegrees{Theta: 45, OX: 0, OY: 0, OZ: 1})\n" + + " result := spatialmath.Compose(a, spatialmath.PoseInverse(a))", + Answer: FormatPose(r4), + Explanation: "Always true: Compose(a, PoseInverse(a)) = zero pose, regardless of orientation.", + InputPoses: map[string]spatialmath.Pose{"a": a3}, + ResultPose: r4, + }, + }, + } +} + +// Level 5: Practical Scenarios +func makeLevel5() Level { + // Scenario 1: End effector + camera offset + endEffector := spatialmath.NewPose( + r3.Vector{X: 500, Y: 0, Z: 300}, + ovd(0, 0, 0, 1), + ) + cameraOffset := spatialmath.NewPose( + r3.Vector{X: 0, Y: 0, Z: 50}, + ovd(180, 0, 0, 1), + ) + objectInCamera := spatialmath.NewPose( + r3.Vector{X: 100, Y: 0, Z: 0}, + ovd(0, 0, 0, 1), + ) + cameraInWorld := spatialmath.Compose(endEffector, cameraOffset) + objectInWorld := spatialmath.Compose(cameraInWorld, objectInCamera) + + // Scenario 2: Object in world, find relative to arm + armPose := spatialmath.NewPose( + r3.Vector{X: 200, Y: 0, Z: 400}, + ovd(90, 0, 0, 1), + ) + objectWorld := spatialmath.NewPose( + r3.Vector{X: 300, Y: 100, Z: 400}, + ovd(0, 0, 0, 1), + ) + objectRelative := spatialmath.Compose(spatialmath.PoseInverse(armPose), objectWorld) + + // Scenario 3: Chain of frames (base -> shoulder -> elbow -> hand) + basePose := spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 100}, ovd(0, 0, 0, 1)) + shoulderOffset := spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 200}, ovd(0, 1, 0, 0)) + elbowOffset := spatialmath.NewPose(r3.Vector{X: 150, Y: 0, Z: 0}, ovd(0, 0, 0, 1)) + handInWorld := spatialmath.Compose(spatialmath.Compose(basePose, shoulderOffset), elbowOffset) + + return Level{ + Number: 5, + Title: "Practical Scenarios", + Description: "Real-world robotics problems using the same Compose/PoseInverse/PoseBetween operations.", + Questions: []Question{ + { + Setup: " // A robot arm's end effector is at this world pose:\n" + + " endEffector := spatialmath.NewPose(\n" + + " r3.Vector{X: 500, Y: 0, Z: 300},\n" + + " &spatialmath.OrientationVectorDegrees{Theta: 0, OX: 0, OY: 0, OZ: 1})\n" + + " // A camera is mounted on the end effector with this offset:\n" + + " cameraOffset := spatialmath.NewPose(\n" + + " r3.Vector{X: 0, Y: 0, Z: 50},\n" + + " &spatialmath.OrientationVectorDegrees{Theta: 180, OX: 0, OY: 0, OZ: 1})\n" + + " // The camera sees an object at this relative pose:\n" + + " objectInCamera := spatialmath.NewPose(\n" + + " r3.Vector{X: 100, Y: 0, Z: 0},\n" + + " &spatialmath.OrientationVectorDegrees{Theta: 0, OX: 0, OY: 0, OZ: 1})\n" + + "\n" + + " // Where is the object in world frame?\n" + + " cameraInWorld := spatialmath.Compose(endEffector, cameraOffset)\n" + + " result := spatialmath.Compose(cameraInWorld, objectInCamera)", + Answer: FormatPose(objectInWorld), + Explanation: "Chain the transforms: world->endEffector->camera->object.\n" + + " Each Compose moves from one frame to the next.", + InputPoses: map[string]spatialmath.Pose{ + "endEffector": endEffector, "cameraInWorld": cameraInWorld, "objectInCamera": objectInCamera, + }, + ResultPose: objectInWorld, + }, + { + Setup: " // An arm is at this world pose:\n" + + " armPose := spatialmath.NewPose(\n" + + " r3.Vector{X: 200, Y: 0, Z: 400},\n" + + " &spatialmath.OrientationVectorDegrees{Theta: 90, OX: 0, OY: 0, OZ: 1})\n" + + " // An object is at this world pose:\n" + + " objectWorld := spatialmath.NewPose(\n" + + " r3.Vector{X: 300, Y: 100, Z: 400},\n" + + " &spatialmath.OrientationVectorDegrees{Theta: 0, OX: 0, OY: 0, OZ: 1})\n" + + "\n" + + " // What is the object's pose RELATIVE to the arm?\n" + + " result := spatialmath.Compose(spatialmath.PoseInverse(armPose), objectWorld)", + Answer: FormatPose(objectRelative), + Explanation: "To express B in A's frame: Compose(PoseInverse(A), B).\n" + + " PoseInverse(arm) 'undoes' the arm's transform, then objectWorld applies.", + InputPoses: map[string]spatialmath.Pose{ + "armPose": armPose, "objectWorld": objectWorld, + }, + ResultPose: objectRelative, + }, + { + Setup: " // A robot arm has joints connected in a chain:\n" + + " basePose := spatialmath.NewPose(\n" + + " r3.Vector{X: 0, Y: 0, Z: 100},\n" + + " &spatialmath.OrientationVectorDegrees{Theta: 0, OX: 0, OY: 0, OZ: 1})\n" + + " shoulderOffset := spatialmath.NewPose(\n" + + " r3.Vector{X: 0, Y: 0, Z: 200},\n" + + " &spatialmath.OrientationVectorDegrees{Theta: 0, OX: 1, OY: 0, OZ: 0})\n" + + " elbowOffset := spatialmath.NewPose(\n" + + " r3.Vector{X: 150, Y: 0, Z: 0},\n" + + " &spatialmath.OrientationVectorDegrees{Theta: 0, OX: 0, OY: 0, OZ: 1})\n" + + "\n" + + " // Where is the hand (after elbow) in world frame?\n" + + " result := spatialmath.Compose(\n" + + " spatialmath.Compose(basePose, shoulderOffset), elbowOffset)", + Answer: FormatPose(handInWorld), + Explanation: "Forward kinematics: chain Compose from base to end effector.\n" + + " Each joint's orientation changes subsequent translation directions.", + InputPoses: map[string]spatialmath.Pose{ + "basePose": basePose, "shoulderOffset": shoulderOffset, "elbowOffset": elbowOffset, + }, + ResultPose: handInWorld, + }, + }, + } +} diff --git a/spatialmath/worksheet/visualize.go b/spatialmath/worksheet/visualize.go new file mode 100644 index 00000000000..508530f41fa --- /dev/null +++ b/spatialmath/worksheet/visualize.go @@ -0,0 +1,91 @@ +package worksheet + +import ( + "fmt" + "sort" + + "github.com/golang/geo/r3" + viz "github.com/viam-labs/motion-tools/client/client" + + "go.viam.com/rdk/spatialmath" +) + +// boxDims is the asymmetric box dimensions (10x20x30) so orientation is visually obvious. +var boxDims = r3.Vector{X: 10, Y: 20, Z: 30} + +// orderedColors are assigned to poses by sorted key order. +var orderedColors = []string{"blue", "green", "yellow", "purple", "cyan"} + +// PoseColorByIndex returns the color for the nth pose (0-indexed). +func PoseColorByIndex(i int) string { + if i < len(orderedColors) { + return orderedColors[i] + } + return "purple" +} + +// vizEnabled tracks whether visualization is available. +var vizEnabled = true + +// DrawInputPoses draws the input poses as colored asymmetric boxes. +func DrawInputPoses(poses map[string]spatialmath.Pose) { + if !vizEnabled { + return + } + if err := viz.RemoveAllSpatialObjects(); err != nil { + //nolint:forbidigo + fmt.Println(" (motion-tools not available, continuing text-only)") + vizEnabled = false + return + } + + // Draw origin reference box in white + originBox, err := spatialmath.NewBox(spatialmath.NewZeroPose(), boxDims, "origin") + if err == nil { + if err := viz.DrawGeometry(originBox, "white"); err != nil { + vizEnabled = false + return + } + } + + names := make([]string, 0, len(poses)) + for name := range poses { + names = append(names, name) + } + sort.Strings(names) + + for i, name := range names { + pose := poses[name] + color := PoseColorByIndex(i) + box, err := spatialmath.NewBox(pose, boxDims, name) + if err != nil { + continue + } + if err := viz.DrawGeometry(box, color); err != nil { + vizEnabled = false + return + } + } +} + +// DrawResult draws the result pose as a red box (additive, does not clear existing objects). +func DrawResult(pose spatialmath.Pose) { + if !vizEnabled { + return + } + box, err := spatialmath.NewBox(pose, boxDims, "result") + if err != nil { + return + } + //nolint:errcheck + viz.DrawGeometry(box, "red") +} + +// ClearVisualization removes all objects from motion-tools. +func ClearVisualization() { + if !vizEnabled { + return + } + //nolint:errcheck + viz.RemoveAllSpatialObjects() +}