From d502979f341a6e34ea06304d2871cc7c309a6a6a Mon Sep 17 00:00:00 2001 From: Dan Gottlieb Date: Tue, 2 Jun 2026 13:44:33 -0400 Subject: [PATCH 01/12] RSDK-13688: Track PerGoal meta information in TestBadSpray1. Output it on test failure. --- motionplan/armplanning/api.go | 54 +++++++++++++++++++++++--- motionplan/armplanning/plan_manager.go | 10 ++++- motionplan/armplanning/real_test.go | 10 ++++- 3 files changed, 65 insertions(+), 9 deletions(-) diff --git a/motionplan/armplanning/api.go b/motionplan/armplanning/api.go index 127386072f8..1ae15b7f74a 100644 --- a/motionplan/armplanning/api.go +++ b/motionplan/armplanning/api.go @@ -192,6 +192,8 @@ type SolutionNodeInfo struct { // PerGoalMeta holds diagnostic data for a single invocation of initRRTSolutions. // Only populated when PlannerOptions.CollectSolutionDiagnostics is true. type PerGoalMeta struct { + StartConfiguration *referenceframe.LinearInputs + ReasonableCost float64 // SolutionNodes contains info about each IK solution node scored and path-checked. SolutionNodes []SolutionNodeInfo // ConstraintFailuresByType maps constraint error strings to the number of IK candidate @@ -201,10 +203,19 @@ type PerGoalMeta struct { // PlanMeta is meta data about plan generation. type PlanMeta struct { - Duration time.Duration - Partial bool - PartialError error + Duration time.Duration + Partial bool + PartialError error + // GoalsProcessed is how many user-defined goals were solved for. GoalsProcessed int + // SubgoalsPerGoal will have size of `GoalsProcessed`. If there are no linear/orientation + // constraints, we do not create any additional subgoals/waypoints. SubgoalsPerGoal in that case + // will be set to 1 for each goal index. Otherwise it will be sent to the number of internal + // waypoints created + 1 (for the final user goal). + SubgoalsPerGoal []int + // SubgoalsProcessed may be non-zero when a plan request has linear/orientation + // constraints. Satisfying those constraints internally creates additional goals. + SubgoalsProcessed int // CollectSolutionDiagnostics is copied from PlannerOptions and gates whether PerGoal is // populated. @@ -215,6 +226,40 @@ type PlanMeta struct { PerGoal []PerGoalMeta } +// OutputToLogger dumps all PerGoal diagnostic data to the logger in a human-readable format. +// Only useful when CollectSolutionDiagnostics was set on the PlannerOptions. +func (meta *PlanMeta) OutputToLogger(logger logging.Logger) { + logger.Infof("PlanMeta: duration=%v partial=%v goalsProcessed=%d subgoalsProcessed=%d subgoalsPerGoal=%v", + meta.Duration, meta.Partial, meta.GoalsProcessed, meta.SubgoalsProcessed, meta.SubgoalsPerGoal) + if meta.PartialError != nil { + logger.Infof(" partialError: %v", meta.PartialError) + } + + if !meta.CollectSolutionDiagnostics { + logger.Infof(" (CollectSolutionDiagnostics was false — no PerGoal data)") + return + } + + logger.Infof(" perGoal entries: %d", len(meta.PerGoal)) + for idx, pg := range meta.PerGoal { + logger.Infof(" [goal %d] startConfig=%v reasonableCost=%.4f solutionNodes=%d constraintFailures=%d", + idx, pg.StartConfiguration, pg.ReasonableCost, len(pg.SolutionNodes), len(pg.ConstraintFailuresByType)) + for failType, count := range pg.ConstraintFailuresByType { + logger.Infof(" constraintFailure %q: %d", failType, count) + } + + for nodeIdx, node := range pg.SolutionNodes { + if node.CheckPathError != nil { + logger.Infof(" [node %d] score=%.4f checkPathError=%v lastGoodInputs=%v inputs=%v", + nodeIdx, node.Score, node.CheckPathError, node.LastGoodInputs, node.Inputs) + } else { + logger.Infof(" [node %d] score=%.4f checkPath=ok inputs=%v", + nodeIdx, node.Score, node.Inputs) + } + } + } +} + // PlanMotion plans a motion from a provided plan request. func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanRequest) (motionplan.Plan, *PlanMeta, error) { logger := parentLogger.Sublogger("mp") @@ -258,6 +303,7 @@ func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanR } trajAsInps, goalsProcessed, err := sfPlanner.planMultiWaypoint(ctx) + meta.GoalsProcessed = goalsProcessed if err != nil { if request.PlannerOptions.ReturnPartialPlan { meta.Partial = true @@ -268,8 +314,6 @@ func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanR } } - meta.GoalsProcessed = goalsProcessed - t, err := motionplan.NewSimplePlanFromTrajectory(trajAsInps, request.FrameSystem) if err != nil { return nil, meta, err diff --git a/motionplan/armplanning/plan_manager.go b/motionplan/armplanning/plan_manager.go index f114c5efa4b..c0334a4e5ab 100644 --- a/motionplan/armplanning/plan_manager.go +++ b/motionplan/armplanning/plan_manager.go @@ -54,6 +54,7 @@ func (pm *planManager) planMultiWaypoint(ctx context.Context) ([]*referenceframe return nil, 0, err } + pm.pc.planMeta.SubgoalsPerGoal = make([]int, len(pm.request.Goals)) for i, g := range pm.request.Goals { if ctx.Err() != nil { return linearTraj, i, err // note: here and below, we return traj because of ReturnPartialPlan @@ -84,6 +85,7 @@ func (pm *planManager) planMultiWaypoint(ctx context.Context) ([]*referenceframe if err != nil { return linearTraj, i, err } + pm.pc.planMeta.SubgoalsPerGoal[i] = len(subGoals) if len(subGoals) > 1 { pm.logger.Debugf("\t generateWaypoint turned into %d subGoals cbirrtAllowed: %v", len(subGoals), cbirrtAllowed) @@ -97,6 +99,7 @@ func (pm *planManager) planMultiWaypoint(ctx context.Context) ([]*referenceframe for subGoalIdx, sg := range subGoals { singleGoalStart := time.Now() newTraj, err := pm.planSingleGoal(ctx, linearTraj[len(linearTraj)-1], sg, cbirrtAllowed) + pm.pc.planMeta.SubgoalsProcessed = subGoalIdx if err != nil { pm.logger.Infof("\t subgoal %d failed after %v with: %v", subGoalIdx, time.Since(singleGoalStart), err) return linearTraj, i, err @@ -329,8 +332,13 @@ func initRRTSolutions(ctx context.Context, psc *planSegmentContext, logger loggi rrt.maps.optNode = goalNodes[0] logger.Debugf("optNode cost: %v", rrt.maps.optNode.cost) + // `defaultOptimalityMultiple` is > 1.0 + reasonableCost := max(.01, goalNodes[0].cost) * defaultOptimalityMultiple + if psc.pc.planMeta.CollectSolutionDiagnostics { perGoal := &psc.pc.planMeta.PerGoal[len(psc.pc.planMeta.PerGoal)-1] + perGoal.StartConfiguration = psc.start + perGoal.ReasonableCost = reasonableCost for _, goalNode := range goalNodes { perGoal.SolutionNodes = append(perGoal.SolutionNodes, SolutionNodeInfo{ Score: goalNode.cost, @@ -341,8 +349,6 @@ func initRRTSolutions(ctx context.Context, psc *planSegmentContext, logger loggi } } - // `defaultOptimalityMultiple` is > 1.0 - reasonableCost := max(.01, goalNodes[0].cost) * defaultOptimalityMultiple for _, solution := range goalNodes { if solution.cost > reasonableCost { // if it's this bad, we don't want for cbirrt or going straight diff --git a/motionplan/armplanning/real_test.go b/motionplan/armplanning/real_test.go index 226151c6537..44f89feaec3 100644 --- a/motionplan/armplanning/real_test.go +++ b/motionplan/armplanning/real_test.go @@ -294,16 +294,22 @@ func TestBadSpray1(t *testing.T) { return } - logger := newChattyMotionPlanTestLogger(t) + logger := logging.NewTestLogger(t) start := time.Now() req, err := ReadRequestFromFile("data/spray-bad1.json") test.That(t, err, test.ShouldBeNil) + req.PlannerOptions.CollectSolutionDiagnostics = true logger.Infof("time to ReadRequestFromFile %v", time.Since(start)) t.Run("basic", func(t *testing.T) { - _, _, err = PlanMotion(context.Background(), logger, req) + // Pass in a `newChattyMotionPlanTestLogger` that disables motion planning INFO/DEBUG logs + // for performance. Lest we timeout due to excessive I/O. + _, meta, err := PlanMotion(context.Background(), newChattyMotionPlanTestLogger(t), req) + if err != nil { + meta.OutputToLogger(logger) + } test.That(t, err, test.ShouldBeNil) }) From ed8e38f447544bbf07d1460d7b271136fa71a589 Mon Sep 17 00:00:00 2001 From: Dan Gottlieb Date: Tue, 2 Jun 2026 16:25:13 -0400 Subject: [PATCH 02/12] ik page for mp server --- motionplan/armplanning/api.go | 5 +- motionplan/armplanning/mpserver/server.go | 410 +++++++++++++++++++--- motionplan/armplanning/plan_manager.go | 1 + motionplan/armplanning/real_test.go | 2 +- motionplan/ik/combined.go | 10 +- 5 files changed, 381 insertions(+), 47 deletions(-) diff --git a/motionplan/armplanning/api.go b/motionplan/armplanning/api.go index 1ae15b7f74a..62b70c18fc3 100644 --- a/motionplan/armplanning/api.go +++ b/motionplan/armplanning/api.go @@ -193,6 +193,7 @@ type SolutionNodeInfo struct { // Only populated when PlannerOptions.CollectSolutionDiagnostics is true. type PerGoalMeta struct { StartConfiguration *referenceframe.LinearInputs + GoalPoses referenceframe.FrameSystemPoses ReasonableCost float64 // SolutionNodes contains info about each IK solution node scored and path-checked. SolutionNodes []SolutionNodeInfo @@ -242,8 +243,8 @@ func (meta *PlanMeta) OutputToLogger(logger logging.Logger) { logger.Infof(" perGoal entries: %d", len(meta.PerGoal)) for idx, pg := range meta.PerGoal { - logger.Infof(" [goal %d] startConfig=%v reasonableCost=%.4f solutionNodes=%d constraintFailures=%d", - idx, pg.StartConfiguration, pg.ReasonableCost, len(pg.SolutionNodes), len(pg.ConstraintFailuresByType)) + logger.Infof(" [goal %d] startConfig=%v goal=%v reasonableCost=%.4f solutionNodes=%d constraintFailures=%d", + idx, pg.StartConfiguration, pg.GoalPoses, pg.ReasonableCost, len(pg.SolutionNodes), len(pg.ConstraintFailuresByType)) for failType, count := range pg.ConstraintFailuresByType { logger.Infof(" constraintFailure %q: %d", failType, count) } diff --git a/motionplan/armplanning/mpserver/server.go b/motionplan/armplanning/mpserver/server.go index 89e54ff1ca5..1c1fa69ee61 100644 --- a/motionplan/armplanning/mpserver/server.go +++ b/motionplan/armplanning/mpserver/server.go @@ -9,6 +9,7 @@ import ( "fmt" "html/template" "net/http" + "net/url" "os" "path/filepath" "runtime" @@ -170,6 +171,32 @@ var detailTmpl = template.Must(template.New("detail").Parse(`

No moving-frame inputs in start state.

{{end}} +

Goals

+{{if .Goals}} +{{range .Goals}} +

Goal {{.Index}} — Inspect IK

+

Start Configuration

+{{if .StartConfig}} + + + {{range .StartConfig}} + + {{end}} +
FrameInputs
{{.Name}}{{.Inputs}}
+{{else}} +

No moving-frame inputs.

+{{end}} +

Goal Poses (World Frame)

+{{if .GoalPoses}} + +{{else}} +

No goal poses.

+{{end}} +{{end}} +{{else}} +

No goals.

+{{end}} +

Frame System

@@ -216,7 +243,9 @@ function runPlanning() { '   Duration: ' + data.duration + '   Goals processed: ' + data.goals_processed + '

'; (data.per_goal || []).forEach((pg, goalIdx) => { - html += '

Goal ' + goalIdx + '

'; + let goalHeader = 'Goal ' + goalIdx; + if (pg.ik_inspect_url) goalHeader += ' — Inspect IK'; + html += '

' + goalHeader + '

'; html += buildSolutionTable('{{.File}}', 'Valid solutions', pg.valid_solutions || [], false); html += buildSolutionTable('{{.File}}', 'checkPath failures', pg.check_path_failures || [], true); if (pg.constraint_failures_by_type && Object.keys(pg.constraint_failures_by_type).length) { @@ -228,10 +257,22 @@ function runPlanning() { } }); div.innerHTML = html; + if (data.trajectory && data.trajectory.length) { + renderPlan('{{.File}}', data.trajectory); + } }) .catch(err => { if (err.name !== 'AbortError') div.textContent = 'Fetch error: ' + err; }); } +function renderPlan(file, trajectory) { + fetch('/render-plan?file=' + encodeURIComponent(file), { + method: 'POST', + headers: {'Content-Type': 'application/json'}, + body: JSON.stringify(trajectory), + }).then(r => { if (!r.ok) r.text().then(msg => console.error('Render error: ' + msg)); }) + .catch(err => console.error('Render error: ' + err)); +} + function buildSolutionTable(file, title, solutions, showError) { if (!solutions.length) return ''; let html = '

' + title + ' (' + solutions.length + ')

'; @@ -261,9 +302,10 @@ function buildSolutionTable(file, title, solutions, showError) { return html; } +// inputs is map[string][]string — values are already full-precision strings from the server. function formatInputs(inputs) { return Object.entries(inputs) - .map(([f, vs]) => f + ': [' + vs.map(v => v.toFixed(4)).join(', ') + ']') + .map(([f, vs]) => f + ': [' + vs.join(', ') + ']') .join('
'); } @@ -293,6 +335,77 @@ function escHtml(s) { `)) +var ikInspectTmpl = template.Must(template.New("ik-inspect").Parse(` + + +IK Inspect — {{.File}} + + + +

IK Inspect

+

{{.File}} — ← Back to Detail

+ + + +

Start Configuration

+{{if .StartConfig}} +
FrameDoFParent
+ + {{range .StartConfig}} + + + + + {{end}} +
FrameInputs
{{.Name}}{{.Inputs}}
+{{else}} +

No moving-frame inputs in start state.

+{{end}} + +

Goal Poses (World Frame)

+{{if .GoalPoses}} + +{{else}} +

No goal poses.

+{{end}} + + + + +`)) + // ---- data types ---- type frameInfo struct { @@ -301,59 +414,96 @@ type frameInfo struct { Parent string } +type frameInputs struct { + Name string + Inputs string +} + +// poseDisplay holds a single frame's pose as a human-readable string for template rendering. +type poseDisplay struct { + Frame string + Pose string +} + +type goalDetail struct { + Index int + StartConfig []frameInputs + GoalPoses []poseDisplay + IKInspectURL string +} + type detailData struct { File string Frames []frameInfo StartInputs []frameInputs + Goals []goalDetail } -type frameInputs struct { - Name string - Inputs string +type ikInspectData struct { + File string + StartConfig []frameInputs + GoalPoses []poseDisplay } type planRunResult struct { - Error string `json:"error,omitempty"` - Steps int `json:"steps,omitempty"` - Duration string `json:"duration,omitempty"` - GoalsProcessed int `json:"goals_processed,omitempty"` - Partial bool `json:"partial,omitempty"` - PartialError string `json:"partial_error,omitempty"` - PerGoal []perGoalResult `json:"per_goal,omitempty"` + Error string `json:"error,omitempty"` + Steps int `json:"steps,omitempty"` + Duration string `json:"duration,omitempty"` + GoalsProcessed int `json:"goals_processed,omitempty"` + Partial bool `json:"partial,omitempty"` + PartialError string `json:"partial_error,omitempty"` + PerGoal []perGoalResult `json:"per_goal,omitempty"` + Trajectory []map[string][]string `json:"trajectory,omitempty"` } type perGoalResult struct { + IKInspectURL string `json:"ik_inspect_url,omitempty"` ValidSolutions []solutionNodeResult `json:"valid_solutions,omitempty"` CheckPathFailures []solutionNodeResult `json:"check_path_failures,omitempty"` ConstraintFailuresByType map[string]int `json:"constraint_failures_by_type,omitempty"` } +// solutionNodeResult uses string-valued inputs to preserve full float64 precision across +// the Go→JSON→JS→JSON→Go round-trip. type solutionNodeResult struct { - Score float64 `json:"score"` - CheckPathError string `json:"check_path_error,omitempty"` - Inputs map[string][]float64 `json:"inputs"` - LastGoodInputs map[string][]float64 `json:"last_good_inputs,omitempty"` + Score float64 `json:"score"` + CheckPathError string `json:"check_path_error,omitempty"` + Inputs map[string][]string `json:"inputs"` + LastGoodInputs map[string][]string `json:"last_good_inputs,omitempty"` } -func linearInputsToFloats(li *referenceframe.LinearInputs) map[string][]float64 { - out := make(map[string][]float64) +// linearInputsToStrings converts LinearInputs to a map of string slices so that float64 values +// are transmitted to the frontend without precision loss. +func linearInputsToStrings(li *referenceframe.LinearInputs) map[string][]string { + out := make(map[string][]string) for frameName, inputs := range li.Items() { if len(inputs) == 0 { continue } - floats := make([]float64, len(inputs)) - copy(floats, inputs) - out[frameName] = floats + strs := make([]string, len(inputs)) + for idx, v := range inputs { + strs[idx] = strconv.FormatFloat(v, 'g', -1, 64) + } + out[frameName] = strs } return out } -func floatsToLinearInputs(data map[string][]float64) *referenceframe.LinearInputs { +// stringsToLinearInputs parses string-valued inputs (as sent from the frontend) back to LinearInputs. +func stringsToLinearInputs(data map[string][]string) (*referenceframe.LinearInputs, error) { li := referenceframe.NewLinearInputs() - for frameName, floats := range data { + for frameName, strs := range data { + floats := make([]float64, len(strs)) + for idx, s := range strs { + v, err := strconv.ParseFloat(s, 64) + if err != nil { + return nil, fmt.Errorf("frame %s index %d: %w", frameName, idx, err) + } + floats[idx] = v + } li.Put(frameName, floats) } - return li + return li, nil } // ---- helpers ---- @@ -407,7 +557,7 @@ func buildStartInputs(cfg referenceframe.FrameSystemInputs) []frameInputs { } parts := make([]string, len(inputs)) for i, v := range inputs { - parts[i] = strconv.FormatFloat(v, 'f', 4, 64) + parts[i] = strconv.FormatFloat(v, 'g', -1, 64) } rows = append(rows, frameInputs{Name: name, Inputs: "[" + strings.Join(parts, ", ") + "]"}) } @@ -415,6 +565,72 @@ func buildStartInputs(cfg referenceframe.FrameSystemInputs) []frameInputs { return rows } +// formatPose formats a Pose as a human-readable string with full float64 precision. +func formatPose(pose spatialmath.Pose) string { + pt := pose.Point() + q := pose.Orientation().Quaternion() + return fmt.Sprintf( + "pos=[%s, %s, %s] quat=[w=%s, i=%s, j=%s, k=%s]", + strconv.FormatFloat(pt.X, 'g', -1, 64), + strconv.FormatFloat(pt.Y, 'g', -1, 64), + strconv.FormatFloat(pt.Z, 'g', -1, 64), + strconv.FormatFloat(q.Real, 'g', -1, 64), + strconv.FormatFloat(q.Imag, 'g', -1, 64), + strconv.FormatFloat(q.Jmag, 'g', -1, 64), + strconv.FormatFloat(q.Kmag, 'g', -1, 64), + ) +} + +// computeGoalPoseMap returns full-precision pose strings for one goal, keyed by frame name, +// transformed into the world frame. +func computeGoalPoseMap(req *armplanning.PlanRequest, goalIdx int) (map[string]string, error) { + if goalIdx < 0 || goalIdx >= len(req.Goals) { + return nil, fmt.Errorf("goal index %d out of range (have %d goals)", goalIdx, len(req.Goals)) + } + poses, err := req.Goals[goalIdx].ComputePoses(context.Background(), req.FrameSystem) + if err != nil { + return nil, err + } + result := make(map[string]string, len(poses)) + for frameName, poseValue := range poses { + poseInWorldFrame := poseValue.Transform( + referenceframe.NewPoseInFrame( + req.FrameSystem.World().Name(), + spatialmath.NewZeroPose())).(*referenceframe.PoseInFrame) + result[frameName] = formatPose(poseInWorldFrame.Pose()) + } + return result, nil +} + +// poseMapToDisplays converts a frame→poseString map into a sorted slice for template rendering. +func poseMapToDisplays(poseMap map[string]string) []poseDisplay { + displays := make([]poseDisplay, 0, len(poseMap)) + for frame, pose := range poseMap { + displays = append(displays, poseDisplay{Frame: frame, Pose: pose}) + } + sort.Slice(displays, func(i, j int) bool { return displays[i].Frame < displays[j].Frame }) + return displays +} + +// frameSystemPosesToMap formats a FrameSystemPoses (already world-frame) as a frame→poseString map. +func frameSystemPosesToMap(poses referenceframe.FrameSystemPoses) map[string]string { + result := make(map[string]string, len(poses)) + for frameName, pif := range poses { + result[frameName] = formatPose(pif.Pose()) + } + return result +} + +// buildIKInspectURL constructs the /ik-inspect URL with start config and goal poses encoded as +// JSON query params so all float64 values survive the round-trip as strings. +func buildIKInspectURL(file string, startConfig *referenceframe.LinearInputs, goalPoseMap map[string]string) string { + startJSON, _ := json.Marshal(linearInputsToStrings(startConfig)) + goalJSON, _ := json.Marshal(goalPoseMap) + return "/ik-inspect?file=" + url.QueryEscape(file) + + "&start_config=" + url.QueryEscape(string(startJSON)) + + "&goal_poses=" + url.QueryEscape(string(goalJSON)) +} + func drawGoalPoses(req *armplanning.PlanRequest) error { var goalPoses []spatialmath.Pose for _, goalPlanState := range req.Goals { @@ -454,7 +670,8 @@ func renderState(relPath string) error { return nil } -func visualizePlan(req *armplanning.PlanRequest, plan motionplan.Plan) error { +// visualizeLinearTrajectory renders a sequence of LinearInputs steps in the visualizer. +func visualizeLinearTrajectory(req *armplanning.PlanRequest, steps []*referenceframe.LinearInputs) error { startInputs := req.StartState.Configuration() if err := viz.RemoveAllSpatialObjects(); err != nil { return err @@ -468,12 +685,12 @@ func visualizePlan(req *armplanning.PlanRequest, plan motionplan.Plan) error { if err := drawGoalPoses(req); err != nil { return err } - for idx := range plan.Path() { + for idx, step := range steps { if idx > 0 { midPoints, err := motionplan.InterpolateSegmentFS( &motionplan.SegmentFS{ - StartConfiguration: plan.Trajectory()[idx-1].ToLinearInputs(), - EndConfiguration: plan.Trajectory()[idx].ToLinearInputs(), + StartConfiguration: steps[idx-1], + EndConfiguration: step, FS: req.FrameSystem, }, 2) if err != nil { @@ -486,7 +703,7 @@ func visualizePlan(req *armplanning.PlanRequest, plan motionplan.Plan) error { time.Sleep(renderFramePeriod) } } - if err := viz.DrawFrameSystem(req.FrameSystem, plan.Trajectory()[idx]); err != nil { + if err := viz.DrawFrameSystem(req.FrameSystem, step.ToFrameSystemInputs()); err != nil { return err } time.Sleep(renderFramePeriod) @@ -494,6 +711,15 @@ func visualizePlan(req *armplanning.PlanRequest, plan motionplan.Plan) error { return nil } +func planTrajectoryToStrings(plan motionplan.Plan) []map[string][]string { + traj := plan.Trajectory() + result := make([]map[string][]string, len(traj)) + for idx := range plan.Path() { + result[idx] = linearInputsToStrings(traj[idx].ToLinearInputs()) + } + return result +} + // ---- handlers ---- func handleIndex(logger logging.Logger) http.HandlerFunc { @@ -522,10 +748,26 @@ func handleDetail(logger logging.Logger) http.HandlerFunc { http.Error(w, fmt.Sprintf("reading plan file: %v", err), http.StatusInternalServerError) return } + startConfig := buildStartInputs(req.StartState.Configuration()) + startLI := req.StartState.Configuration().ToLinearInputs() + goals := make([]goalDetail, len(req.Goals)) + for idx := range req.Goals { + poseMap, err := computeGoalPoseMap(req, idx) + if err != nil { + logger.Warnf("computing goal poses for goal %d: %v", idx, err) + } + goals[idx] = goalDetail{ + Index: idx, + StartConfig: startConfig, + GoalPoses: poseMapToDisplays(poseMap), + IKInspectURL: buildIKInspectURL(file, startLI, poseMap), + } + } data := detailData{ File: file, Frames: buildFrameInfo(req.FrameSystem), - StartInputs: buildStartInputs(req.StartState.Configuration()), + StartInputs: startConfig, + Goals: goals, } w.Header().Set("Content-Type", "text/html; charset=utf-8") if err := detailTmpl.Execute(w, data); err != nil { @@ -534,6 +776,47 @@ func handleDetail(logger logging.Logger) http.HandlerFunc { } } +func handleIKInspect(logger logging.Logger) http.HandlerFunc { + return func(w http.ResponseWriter, r *http.Request) { + file := r.URL.Query().Get("file") + if file == "" { + http.Error(w, "missing file parameter", http.StatusBadRequest) + return + } + var startConfigStrings map[string][]string + if sc := r.URL.Query().Get("start_config"); sc != "" { + if err := json.Unmarshal([]byte(sc), &startConfigStrings); err != nil { + http.Error(w, fmt.Sprintf("parsing start_config: %v", err), http.StatusBadRequest) + return + } + } + var goalPoseStrings map[string]string + if gp := r.URL.Query().Get("goal_poses"); gp != "" { + if err := json.Unmarshal([]byte(gp), &goalPoseStrings); err != nil { + http.Error(w, fmt.Sprintf("parsing goal_poses: %v", err), http.StatusBadRequest) + return + } + } + startConfig := make([]frameInputs, 0, len(startConfigStrings)) + for frameName, vals := range startConfigStrings { + startConfig = append(startConfig, frameInputs{ + Name: frameName, + Inputs: "[" + strings.Join(vals, ", ") + "]", + }) + } + sort.Slice(startConfig, func(i, j int) bool { return startConfig[i].Name < startConfig[j].Name }) + data := ikInspectData{ + File: file, + StartConfig: startConfig, + GoalPoses: poseMapToDisplays(goalPoseStrings), + } + w.Header().Set("Content-Type", "text/html; charset=utf-8") + if err := ikInspectTmpl.Execute(w, data); err != nil { + logger.Errorf("rendering ik-inspect: %v", err) + } + } +} + func handlePlanRun(logger logging.Logger) http.HandlerFunc { return func(w http.ResponseWriter, r *http.Request) { file := r.URL.Query().Get("file") @@ -571,10 +854,6 @@ func handlePlanRun(logger logging.Logger) http.HandlerFunc { return } - if vizErr := visualizePlan(req, plan); vizErr != nil { - logger.Warnf("visualization failed (motion-tools server may not be running): %v", vizErr) - } - result := planRunResult{ Steps: len(plan.Path()), Duration: meta.Duration.String(), @@ -585,18 +864,20 @@ func handlePlanRun(logger logging.Logger) http.HandlerFunc { result.PartialError = meta.PartialError.Error() } for _, pg := range meta.PerGoal { + poseMap := frameSystemPosesToMap(pg.GoalPoses) pgResult := perGoalResult{ + IKInspectURL: buildIKInspectURL(file, pg.StartConfiguration, poseMap), ConstraintFailuresByType: pg.ConstraintFailuresByType, } for _, sn := range pg.SolutionNodes { row := solutionNodeResult{ Score: sn.Score, - Inputs: linearInputsToFloats(sn.Inputs), + Inputs: linearInputsToStrings(sn.Inputs), } if sn.CheckPathError != nil { row.CheckPathError = sn.CheckPathError.Error() if sn.LastGoodInputs != nil { - row.LastGoodInputs = linearInputsToFloats(sn.LastGoodInputs) + row.LastGoodInputs = linearInputsToStrings(sn.LastGoodInputs) } pgResult.CheckPathFailures = append(pgResult.CheckPathFailures, row) } else { @@ -605,6 +886,7 @@ func handlePlanRun(logger logging.Logger) http.HandlerFunc { } result.PerGoal = append(result.PerGoal, pgResult) } + result.Trajectory = planTrajectoryToStrings(plan) writeJSON(w, result) } @@ -622,12 +904,16 @@ func handleRenderSolution(logger logging.Logger) http.HandlerFunc { http.Error(w, fmt.Sprintf("reading plan file: %v", err), http.StatusInternalServerError) return } - var inputFloats map[string][]float64 - if err := json.NewDecoder(r.Body).Decode(&inputFloats); err != nil { + var inputStrings map[string][]string + if err := json.NewDecoder(r.Body).Decode(&inputStrings); err != nil { http.Error(w, fmt.Sprintf("decoding inputs: %v", err), http.StatusBadRequest) return } - li := floatsToLinearInputs(inputFloats) + li, err := stringsToLinearInputs(inputStrings) + if err != nil { + http.Error(w, fmt.Sprintf("parsing inputs: %v", err), http.StatusBadRequest) + return + } startInputs := req.StartState.Configuration() if err := viz.RemoveAllSpatialObjects(); err != nil { http.Error(w, err.Error(), http.StatusInternalServerError) @@ -659,12 +945,16 @@ func handleRenderShadows(logger logging.Logger) http.HandlerFunc { http.Error(w, fmt.Sprintf("reading plan file: %v", err), http.StatusInternalServerError) return } - var inputFloats map[string][]float64 - if err := json.NewDecoder(r.Body).Decode(&inputFloats); err != nil { + var inputStrings map[string][]string + if err := json.NewDecoder(r.Body).Decode(&inputStrings); err != nil { http.Error(w, fmt.Sprintf("decoding inputs: %v", err), http.StatusBadRequest) return } - end := floatsToLinearInputs(inputFloats) + end, err := stringsToLinearInputs(inputStrings) + if err != nil { + http.Error(w, fmt.Sprintf("parsing inputs: %v", err), http.StatusBadRequest) + return + } startInputs := req.StartState.Configuration() start := startInputs.ToLinearInputs() @@ -781,6 +1071,38 @@ func writeJSON(w http.ResponseWriter, v any) { } } +func handleRenderPlan(logger logging.Logger) http.HandlerFunc { + return func(w http.ResponseWriter, r *http.Request) { + file := r.URL.Query().Get("file") + if file == "" { + http.Error(w, "missing file parameter", http.StatusBadRequest) + return + } + req, err := armplanning.ReadRequestFromFile(filepath.Join(rdkRoot, file)) + if err != nil { + http.Error(w, fmt.Sprintf("reading plan file: %v", err), http.StatusInternalServerError) + return + } + var trajStrings []map[string][]string + if err := json.NewDecoder(r.Body).Decode(&trajStrings); err != nil { + http.Error(w, fmt.Sprintf("decoding trajectory: %v", err), http.StatusBadRequest) + return + } + steps := make([]*referenceframe.LinearInputs, len(trajStrings)) + for idx, step := range trajStrings { + li, err := stringsToLinearInputs(step) + if err != nil { + http.Error(w, fmt.Sprintf("parsing step %d: %v", idx, err), http.StatusBadRequest) + return + } + steps[idx] = li + } + if err := visualizeLinearTrajectory(req, steps); err != nil { + logger.Warnf("visualization failed (motion-tools server may not be running): %v", err) + } + } +} + func handleRenderStart(logger logging.Logger) http.HandlerFunc { return func(w http.ResponseWriter, r *http.Request) { file := r.URL.Query().Get("file") @@ -803,7 +1125,9 @@ func RunServer() error { http.HandleFunc("/", handleIndex(logger)) http.HandleFunc("/detail", handleDetail(logger)) + http.HandleFunc("/ik-inspect", handleIKInspect(logger)) http.HandleFunc("/plan/run", handlePlanRun(logger)) + http.HandleFunc("/render-plan", handleRenderPlan(logger)) http.HandleFunc("/render-start", handleRenderStart(logger)) http.HandleFunc("/render-solution", handleRenderSolution(logger)) http.HandleFunc("/render-shadows", handleRenderShadows(logger)) diff --git a/motionplan/armplanning/plan_manager.go b/motionplan/armplanning/plan_manager.go index c0334a4e5ab..ccd08fd4f0b 100644 --- a/motionplan/armplanning/plan_manager.go +++ b/motionplan/armplanning/plan_manager.go @@ -338,6 +338,7 @@ func initRRTSolutions(ctx context.Context, psc *planSegmentContext, logger loggi if psc.pc.planMeta.CollectSolutionDiagnostics { perGoal := &psc.pc.planMeta.PerGoal[len(psc.pc.planMeta.PerGoal)-1] perGoal.StartConfiguration = psc.start + perGoal.GoalPoses = psc.goal perGoal.ReasonableCost = reasonableCost for _, goalNode := range goalNodes { perGoal.SolutionNodes = append(perGoal.SolutionNodes, SolutionNodeInfo{ diff --git a/motionplan/armplanning/real_test.go b/motionplan/armplanning/real_test.go index 44f89feaec3..7ae9cd03029 100644 --- a/motionplan/armplanning/real_test.go +++ b/motionplan/armplanning/real_test.go @@ -307,7 +307,7 @@ func TestBadSpray1(t *testing.T) { // Pass in a `newChattyMotionPlanTestLogger` that disables motion planning INFO/DEBUG logs // for performance. Lest we timeout due to excessive I/O. _, meta, err := PlanMotion(context.Background(), newChattyMotionPlanTestLogger(t), req) - if err != nil { + if err != nil || true { meta.OutputToLogger(logger) } test.That(t, err, test.ShouldBeNil) diff --git a/motionplan/ik/combined.go b/motionplan/ik/combined.go index b8d588ab4f0..5426328e7d7 100644 --- a/motionplan/ik/combined.go +++ b/motionplan/ik/combined.go @@ -60,15 +60,23 @@ func (ik *CombinedIK) Solve(ctx context.Context, metas := []SeedSolveMetaData{} var solveResultLock sync.Mutex - for _, solver := range ik.solvers { + for idx, solver := range ik.solvers { thisSolver := solver myseed := rseed rseed++ activeSolvers.Add(1) + // fmt.Println("Spinning solver:", idx) utils.PanicCapturingGo(func() { defer activeSolvers.Done() + _ = idx + // if idx != 5 { + // select { + // case <-ctx.Done(): + // case <-time.After(time.Second): + // } + // } n, m, err := thisSolver.Solve(ctx, retChan, totalAttempts, seeds, limits, costFunc, myseed) solveResultLock.Lock() From 08c9263ba35cbec1e175d132595f8c5bb8a94713 Mon Sep 17 00:00:00 2001 From: Dan Gottlieb Date: Tue, 2 Jun 2026 18:59:33 -0400 Subject: [PATCH 03/12] render interruption --- motionplan/armplanning/mpserver/server.go | 50 +++++++++++++++++++++-- 1 file changed, 46 insertions(+), 4 deletions(-) diff --git a/motionplan/armplanning/mpserver/server.go b/motionplan/armplanning/mpserver/server.go index 1c1fa69ee61..ec33f495e3d 100644 --- a/motionplan/armplanning/mpserver/server.go +++ b/motionplan/armplanning/mpserver/server.go @@ -16,6 +16,7 @@ import ( "sort" "strconv" "strings" + "sync" "time" viz "github.com/viam-labs/motion-tools/client/client" @@ -54,6 +55,33 @@ const ( shadowFramePeriod = 100 * time.Millisecond ) +// ---- render coordination ---- +// +// All rendering targets a single shared visualizer (the package-level `viz` +// client), so only one render may own it at a time. Renders are driven by +// independent HTTP requests that may originate from different pages (e.g. the +// detail page's trajectory playback and the IK-inspect page's "Render Start + +// Goals" button), so coordination has to live here on the server rather than in +// any single page's JavaScript. beginRender cancels whatever render is currently +// in flight and returns a context that the next render will cancel in turn; +// long-running renders (trajectory playback, shadows) must check it and bail out +// promptly once superseded. +var ( + renderMu sync.Mutex + renderCancel context.CancelFunc +) + +func beginRender() context.Context { + renderMu.Lock() + defer renderMu.Unlock() + if renderCancel != nil { + renderCancel() + } + ctx, cancel := context.WithCancel(context.Background()) + renderCancel = cancel + return ctx +} + // ---- templates ---- var indexTmpl = template.Must(template.New("index").Parse(` @@ -671,7 +699,8 @@ func renderState(relPath string) error { } // visualizeLinearTrajectory renders a sequence of LinearInputs steps in the visualizer. -func visualizeLinearTrajectory(req *armplanning.PlanRequest, steps []*referenceframe.LinearInputs) error { +// It bails out early (without error) if ctx is cancelled by a newer render. +func visualizeLinearTrajectory(ctx context.Context, req *armplanning.PlanRequest, steps []*referenceframe.LinearInputs) error { startInputs := req.StartState.Configuration() if err := viz.RemoveAllSpatialObjects(); err != nil { return err @@ -686,6 +715,9 @@ func visualizeLinearTrajectory(req *armplanning.PlanRequest, steps []*referencef return err } for idx, step := range steps { + if ctx.Err() != nil { + return nil + } if idx > 0 { midPoints, err := motionplan.InterpolateSegmentFS( &motionplan.SegmentFS{ @@ -697,6 +729,9 @@ func visualizeLinearTrajectory(req *armplanning.PlanRequest, steps []*referencef return err } for _, mp := range midPoints { + if ctx.Err() != nil { + return nil + } if err := viz.DrawFrameSystem(req.FrameSystem, mp.ToFrameSystemInputs()); err != nil { return err } @@ -914,6 +949,7 @@ func handleRenderSolution(logger logging.Logger) http.HandlerFunc { http.Error(w, fmt.Sprintf("parsing inputs: %v", err), http.StatusBadRequest) return } + beginRender() startInputs := req.StartState.Configuration() if err := viz.RemoveAllSpatialObjects(); err != nil { http.Error(w, err.Error(), http.StatusInternalServerError) @@ -955,6 +991,7 @@ func handleRenderShadows(logger logging.Logger) http.HandlerFunc { http.Error(w, fmt.Sprintf("parsing inputs: %v", err), http.StatusBadRequest) return } + ctx := beginRender() startInputs := req.StartState.Configuration() start := startInputs.ToLinearInputs() @@ -979,7 +1016,7 @@ func handleRenderShadows(logger logging.Logger) http.HandlerFunc { http.Error(w, fmt.Sprintf("interpolating: %v", err), http.StatusInternalServerError) return } - if err := drawShadows(req.FrameSystem, midPoints); err != nil { + if err := drawShadows(ctx, req.FrameSystem, midPoints); err != nil { http.Error(w, fmt.Sprintf("drawing shadows: %v", err), http.StatusInternalServerError) return } @@ -1014,7 +1051,7 @@ func interpolateShadows( // drawShadows draws each interpolated configuration as a static "shadow" so the user can see the // full straight-line path at once. Only frames with DoF (or descendants of moving frames) get // shadows. Colors alternate per step to make ordering visible. -func drawShadows(fs *referenceframe.FrameSystem, configs []*referenceframe.LinearInputs) error { +func drawShadows(ctx context.Context, fs *referenceframe.FrameSystem, configs []*referenceframe.LinearInputs) error { isMovingFrame := func(frameName string) bool { frame := fs.Frame(frameName) if frame == nil { @@ -1035,6 +1072,9 @@ func drawShadows(fs *referenceframe.FrameSystem, configs []*referenceframe.Linea shadowColors := []string{"blue", "red"} for idx, cfg := range configs { + if ctx.Err() != nil { + return nil + } gifs, err := referenceframe.FrameSystemGeometries(fs, cfg.ToFrameSystemInputs()) if err != nil { return err @@ -1097,7 +1137,8 @@ func handleRenderPlan(logger logging.Logger) http.HandlerFunc { } steps[idx] = li } - if err := visualizeLinearTrajectory(req, steps); err != nil { + ctx := beginRender() + if err := visualizeLinearTrajectory(ctx, req, steps); err != nil { logger.Warnf("visualization failed (motion-tools server may not be running): %v", err) } } @@ -1110,6 +1151,7 @@ func handleRenderStart(logger logging.Logger) http.HandlerFunc { http.Error(w, "missing file parameter", http.StatusBadRequest) return } + beginRender() if err := renderState(file); err != nil { http.Error(w, err.Error(), http.StatusInternalServerError) return From f2c2a4c0552825715e55d59a2ee0319c82627842 Mon Sep 17 00:00:00 2001 From: Dan Gottlieb Date: Wed, 3 Jun 2026 13:30:59 -0400 Subject: [PATCH 04/12] clean up IK inspection API boundary --- motionplan/armplanning/mpserver/ik.go | 42 +++ motionplan/armplanning/mpserver/server.go | 313 +++++++++++++++++++--- 2 files changed, 313 insertions(+), 42 deletions(-) create mode 100644 motionplan/armplanning/mpserver/ik.go diff --git a/motionplan/armplanning/mpserver/ik.go b/motionplan/armplanning/mpserver/ik.go new file mode 100644 index 00000000000..3fb9cde14cb --- /dev/null +++ b/motionplan/armplanning/mpserver/ik.go @@ -0,0 +1,42 @@ +package mpserver + +import ( + "context" + "errors" + + "go.viam.com/rdk/logging" + "go.viam.com/rdk/referenceframe" +) + +// IKInspectCell describes a single IK solution emitted by one nlopt thread, scored and validated the +// same way getSolutions would score and validate it. +type IKInspectCell struct { + // Cost is the configuration-distance cost of moving from the start configuration to this + // solution — the same "score" reported in the per-goal solution tables. + Cost float64 + // GoalDist is the IK solver's own residual: how close the solution's pose is to the goal pose. + GoalDist float64 + // Exact is true when the solver considered the goal met (GoalDist below the goal threshold). + Exact bool + // Inputs is the solution configuration. + Inputs *referenceframe.LinearInputs + + // Valid is true when the configuration itself passes all state constraints (no self-collision, + // no obstacle collision, within bounds, ...). When false, StateError explains why. + Valid bool + StateError error + + // CheckPathOK is true when the straight-line interpolation from the start configuration to this + // solution passes all constraints. Only meaningful when Valid is true. When false, CheckPathError + // explains why. + CheckPathOK bool + CheckPathError error +} + +type IKInspectTable struct { + Threads [][]IKInspectCell +} + +func InspectIK(ctx context.Context, logger logging.Logger, fs *referenceframe.FrameSystem, goal referenceframe.FrameSystemPoses, numSolutions int) (*IKInspectTable, error) { + return nil, errors.New("unimplemented") +} diff --git a/motionplan/armplanning/mpserver/server.go b/motionplan/armplanning/mpserver/server.go index ec33f495e3d..3dbd96918ee 100644 --- a/motionplan/armplanning/mpserver/server.go +++ b/motionplan/armplanning/mpserver/server.go @@ -19,6 +19,7 @@ import ( "sync" "time" + "github.com/golang/geo/r3" viz "github.com/viam-labs/motion-tools/client/client" "go.viam.com/rdk/logging" @@ -391,6 +392,13 @@ var ikInspectTmpl = template.Must(template.New("ik-inspect").Parse(` @@ -398,6 +406,9 @@ var ikInspectTmpl = template.Must(template.New("ik-inspect").Parse(`{{.File}} — ← Back to Detail

+  + +

Start Configuration

{{if .StartConfig}} @@ -424,11 +435,82 @@ var ikInspectTmpl = template.Must(template.New("ik-inspect").Parse(` +const GOAL_POSES = {{.GoalPosesJSON}}; + function renderStartAndGoals() { fetch('/render-start?file=' + encodeURIComponent('{{.File}}')) .then(r => { if (!r.ok) r.text().then(msg => console.error('Render error: ' + msg)); }) .catch(err => console.error('Render error: ' + err)); } + +let ikInspectAbort = null; + +function runIKInspect() { + if (ikInspectAbort) ikInspectAbort.abort(); + ikInspectAbort = new AbortController(); + const div = document.getElementById('ik-result'); + div.textContent = 'Running IK inspection…'; + fetch('/ik-inspect/run?file=' + encodeURIComponent('{{.File}}') + + '&goal_poses=' + encodeURIComponent(JSON.stringify(GOAL_POSES)), + { signal: ikInspectAbort.signal }) + .then(r => r.json()) + .then(data => { + if (data.error) { + div.innerHTML = '
Error: ' + escHtml(data.error) + '
'; + return; + } + div.innerHTML = buildIKTable(data.threads || []); + }) + .catch(err => { if (err.name !== 'AbortError') div.textContent = 'Fetch error: ' + err; }); +} + +// threads is column-major: threads[col] is the ordered list of solutions thread col emitted. +function buildIKTable(threads) { + const legend = '

' + + 'valid + checkPath ok' + + 'valid, checkPath failed' + + 'invalid (e.g. collision)' + + '

'; + + let maxRows = 0; + for (const col of threads) maxRows = Math.max(maxRows, col.length); + if (maxRows === 0) return legend + '

No IK solutions were emitted.

'; + + let html = legend + ''; + for (let c = 0; c < threads.length; c++) html += ''; + html += ''; + for (let row = 0; row < maxRows; row++) { + html += ''; + for (let c = 0; c < threads.length; c++) { + const cell = threads[c][row]; + if (!cell) { html += ''; continue; } + html += renderIKCell(cell); + } + html += ''; + } + html += '
thread ' + c + '
' + row + '
'; + return html; +} + +function renderIKCell(cell) { + let cls = 'cell-red'; + if (cell.valid && cell.check_path_ok) cls = 'cell-green'; + else if (cell.valid) cls = 'cell-yellow'; + + const tip = []; + tip.push('cost: ' + cell.cost); + tip.push('goalDist: ' + cell.goal_dist + (cell.exact ? ' (exact)' : '')); + if (cell.state_error) tip.push('invalid: ' + cell.state_error); + if (cell.check_path_error) tip.push('checkPath: ' + cell.check_path_error); + + const goalDist = (typeof cell.goal_dist === 'number') ? cell.goal_dist.toExponential(2) : cell.goal_dist; + const inner = '' + cell.cost.toFixed(4) + '
d=' + goalDist + ''; + return '' + inner + ''; +} + +function escHtml(s) { + return String(s).replace(/&/g,'&').replace(//g,'>'); +} @@ -468,19 +550,20 @@ type detailData struct { } type ikInspectData struct { - File string - StartConfig []frameInputs - GoalPoses []poseDisplay + File string + StartConfig []frameInputs + GoalPoses []poseDisplay + GoalPosesJSON template.JS } type planRunResult struct { - Error string `json:"error,omitempty"` - Steps int `json:"steps,omitempty"` - Duration string `json:"duration,omitempty"` - GoalsProcessed int `json:"goals_processed,omitempty"` - Partial bool `json:"partial,omitempty"` - PartialError string `json:"partial_error,omitempty"` - PerGoal []perGoalResult `json:"per_goal,omitempty"` + Error string `json:"error,omitempty"` + Steps int `json:"steps,omitempty"` + Duration string `json:"duration,omitempty"` + GoalsProcessed int `json:"goals_processed,omitempty"` + Partial bool `json:"partial,omitempty"` + PartialError string `json:"partial_error,omitempty"` + PerGoal []perGoalResult `json:"per_goal,omitempty"` Trajectory []map[string][]string `json:"trajectory,omitempty"` } @@ -500,6 +583,26 @@ type solutionNodeResult struct { LastGoodInputs map[string][]string `json:"last_good_inputs,omitempty"` } +// ikInspectRunResult is the JSON payload for the IK-inspect table. Threads is column-major: +// Threads[i] is the ordered list of solutions nlopt thread i emitted. +type ikInspectRunResult struct { + Error string `json:"error,omitempty"` + Threads [][]ikInspectCellResult `json:"threads,omitempty"` +} + +// ikInspectCellResult is one solution emitted by one thread. Inputs use string-valued floats to +// preserve precision, matching solutionNodeResult. +type ikInspectCellResult struct { + Cost float64 `json:"cost"` + GoalDist float64 `json:"goal_dist"` + Exact bool `json:"exact"` + Inputs map[string][]string `json:"inputs,omitempty"` + Valid bool `json:"valid"` + StateError string `json:"state_error,omitempty"` + CheckPathOK bool `json:"check_path_ok"` + CheckPathError string `json:"check_path_error,omitempty"` +} + // linearInputsToStrings converts LinearInputs to a map of string slices so that float64 values // are transmitted to the frontend without precision loss. func linearInputsToStrings(li *referenceframe.LinearInputs) map[string][]string { @@ -593,25 +696,75 @@ func buildStartInputs(cfg referenceframe.FrameSystemInputs) []frameInputs { return rows } -// formatPose formats a Pose as a human-readable string with full float64 precision. -func formatPose(pose spatialmath.Pose) string { +// poseComponents holds the 7 scalar components of a pose as float64 strings. +// Using strings preserves full float64 precision across the Go→JSON→JS→JSON→Go round-trip, +// and prevents JavaScript from silently converting component values to numbers. +type poseComponents struct { + X string `json:"x"` + Y string `json:"y"` + Z string `json:"z"` + W string `json:"w"` + I string `json:"i"` + J string `json:"j"` + K string `json:"k"` +} + +func poseToComponents(pose spatialmath.Pose) poseComponents { pt := pose.Point() q := pose.Orientation().Quaternion() - return fmt.Sprintf( - "pos=[%s, %s, %s] quat=[w=%s, i=%s, j=%s, k=%s]", - strconv.FormatFloat(pt.X, 'g', -1, 64), - strconv.FormatFloat(pt.Y, 'g', -1, 64), - strconv.FormatFloat(pt.Z, 'g', -1, 64), - strconv.FormatFloat(q.Real, 'g', -1, 64), - strconv.FormatFloat(q.Imag, 'g', -1, 64), - strconv.FormatFloat(q.Jmag, 'g', -1, 64), - strconv.FormatFloat(q.Kmag, 'g', -1, 64), - ) + return poseComponents{ + X: strconv.FormatFloat(pt.X, 'g', -1, 64), + Y: strconv.FormatFloat(pt.Y, 'g', -1, 64), + Z: strconv.FormatFloat(pt.Z, 'g', -1, 64), + W: strconv.FormatFloat(q.Real, 'g', -1, 64), + I: strconv.FormatFloat(q.Imag, 'g', -1, 64), + J: strconv.FormatFloat(q.Jmag, 'g', -1, 64), + K: strconv.FormatFloat(q.Kmag, 'g', -1, 64), + } } -// computeGoalPoseMap returns full-precision pose strings for one goal, keyed by frame name, -// transformed into the world frame. -func computeGoalPoseMap(req *armplanning.PlanRequest, goalIdx int) (map[string]string, error) { +func componentsToSpatialPose(pc poseComponents) (spatialmath.Pose, error) { + parse := func(label, s string) (float64, error) { + v, err := strconv.ParseFloat(s, 64) + if err != nil { + return 0, fmt.Errorf("parsing %s: %w", label, err) + } + return v, nil + } + x, err := parse("x", pc.X) + if err != nil { + return nil, err + } + y, err := parse("y", pc.Y) + if err != nil { + return nil, err + } + z, err := parse("z", pc.Z) + if err != nil { + return nil, err + } + w, err := parse("w", pc.W) + if err != nil { + return nil, err + } + qi, err := parse("i", pc.I) + if err != nil { + return nil, err + } + qj, err := parse("j", pc.J) + if err != nil { + return nil, err + } + qk, err := parse("k", pc.K) + if err != nil { + return nil, err + } + return spatialmath.NewPose(r3.Vector{X: x, Y: y, Z: z}, &spatialmath.Quaternion{Real: w, Imag: qi, Jmag: qj, Kmag: qk}), nil +} + +// computeGoalPoseMap returns the poses for one goal, keyed by frame name, transformed into the +// world frame and encoded as poseComponents (string-valued scalars). +func computeGoalPoseMap(req *armplanning.PlanRequest, goalIdx int) (map[string]poseComponents, error) { if goalIdx < 0 || goalIdx >= len(req.Goals) { return nil, fmt.Errorf("goal index %d out of range (have %d goals)", goalIdx, len(req.Goals)) } @@ -619,39 +772,43 @@ func computeGoalPoseMap(req *armplanning.PlanRequest, goalIdx int) (map[string]s if err != nil { return nil, err } - result := make(map[string]string, len(poses)) + result := make(map[string]poseComponents, len(poses)) for frameName, poseValue := range poses { poseInWorldFrame := poseValue.Transform( referenceframe.NewPoseInFrame( req.FrameSystem.World().Name(), spatialmath.NewZeroPose())).(*referenceframe.PoseInFrame) - result[frameName] = formatPose(poseInWorldFrame.Pose()) + result[frameName] = poseToComponents(poseInWorldFrame.Pose()) } return result, nil } -// poseMapToDisplays converts a frame→poseString map into a sorted slice for template rendering. -func poseMapToDisplays(poseMap map[string]string) []poseDisplay { +// poseMapToDisplays converts a frame→poseComponents map into a sorted slice for template rendering. +func poseMapToDisplays(poseMap map[string]poseComponents) []poseDisplay { displays := make([]poseDisplay, 0, len(poseMap)) - for frame, pose := range poseMap { - displays = append(displays, poseDisplay{Frame: frame, Pose: pose}) + for frame, pc := range poseMap { + displays = append(displays, poseDisplay{ + Frame: frame, + Pose: fmt.Sprintf("pos=[%s, %s, %s] quat=[w=%s, i=%s, j=%s, k=%s]", pc.X, pc.Y, pc.Z, pc.W, pc.I, pc.J, pc.K), + }) } sort.Slice(displays, func(i, j int) bool { return displays[i].Frame < displays[j].Frame }) return displays } -// frameSystemPosesToMap formats a FrameSystemPoses (already world-frame) as a frame→poseString map. -func frameSystemPosesToMap(poses referenceframe.FrameSystemPoses) map[string]string { - result := make(map[string]string, len(poses)) +// frameSystemPosesToMap encodes a FrameSystemPoses (already world-frame) as a frame→poseComponents map. +func frameSystemPosesToMap(poses referenceframe.FrameSystemPoses) map[string]poseComponents { + result := make(map[string]poseComponents, len(poses)) for frameName, pif := range poses { - result[frameName] = formatPose(pif.Pose()) + result[frameName] = poseToComponents(pif.Pose()) } return result } // buildIKInspectURL constructs the /ik-inspect URL with start config and goal poses encoded as -// JSON query params so all float64 values survive the round-trip as strings. -func buildIKInspectURL(file string, startConfig *referenceframe.LinearInputs, goalPoseMap map[string]string) string { +// JSON query params. All float64 values are represented as strings so they survive the +// Go→JSON→JS→JSON→Go round-trip without any numeric conversion. +func buildIKInspectURL(file string, startConfig *referenceframe.LinearInputs, goalPoseMap map[string]poseComponents) string { startJSON, _ := json.Marshal(linearInputsToStrings(startConfig)) goalJSON, _ := json.Marshal(goalPoseMap) return "/ik-inspect?file=" + url.QueryEscape(file) + @@ -825,9 +982,9 @@ func handleIKInspect(logger logging.Logger) http.HandlerFunc { return } } - var goalPoseStrings map[string]string + var goalPoseMap map[string]poseComponents if gp := r.URL.Query().Get("goal_poses"); gp != "" { - if err := json.Unmarshal([]byte(gp), &goalPoseStrings); err != nil { + if err := json.Unmarshal([]byte(gp), &goalPoseMap); err != nil { http.Error(w, fmt.Sprintf("parsing goal_poses: %v", err), http.StatusBadRequest) return } @@ -840,10 +997,12 @@ func handleIKInspect(logger logging.Logger) http.HandlerFunc { }) } sort.Slice(startConfig, func(i, j int) bool { return startConfig[i].Name < startConfig[j].Name }) + goalPosesJSONBytes, _ := json.Marshal(goalPoseMap) data := ikInspectData{ - File: file, - StartConfig: startConfig, - GoalPoses: poseMapToDisplays(goalPoseStrings), + File: file, + StartConfig: startConfig, + GoalPoses: poseMapToDisplays(goalPoseMap), + GoalPosesJSON: template.JS(goalPosesJSONBytes), //nolint:gosec } w.Header().Set("Content-Type", "text/html; charset=utf-8") if err := ikInspectTmpl.Execute(w, data); err != nil { @@ -852,6 +1011,75 @@ func handleIKInspect(logger logging.Logger) http.HandlerFunc { } } +func handleIKInspectRun(logger logging.Logger) http.HandlerFunc { + return func(w http.ResponseWriter, r *http.Request) { + file := r.URL.Query().Get("file") + if file == "" { + writeJSON(w, ikInspectRunResult{Error: "missing file parameter"}) + return + } + var goalPoseMap map[string]poseComponents + if gp := r.URL.Query().Get("goal_poses"); gp != "" { + if err := json.Unmarshal([]byte(gp), &goalPoseMap); err != nil { + writeJSON(w, ikInspectRunResult{Error: fmt.Sprintf("parsing goal_poses: %v", err)}) + return + } + } + if len(goalPoseMap) == 0 { + writeJSON(w, ikInspectRunResult{Error: "goal_poses is required"}) + return + } + req, err := armplanning.ReadRequestFromFile(filepath.Join(rdkRoot, file)) + if err != nil { + writeJSON(w, ikInspectRunResult{Error: err.Error()}) + return + } + worldFrameName := req.FrameSystem.World().Name() + goalPoses := make(referenceframe.FrameSystemPoses, len(goalPoseMap)) + for frameName, pc := range goalPoseMap { + pose, err := componentsToSpatialPose(pc) + if err != nil { + writeJSON(w, ikInspectRunResult{Error: fmt.Sprintf("parsing goal pose for %q: %v", frameName, err)}) + return + } + goalPoses[frameName] = referenceframe.NewPoseInFrame(worldFrameName, pose) + } + + armplanning.ClearSeedCache() + result, err := InspectIK(r.Context(), logger.Sublogger("ik-inspect"), req.FrameSystem, goalPoses, 10) + if err != nil { + writeJSON(w, ikInspectRunResult{Error: err.Error()}) + return + } + + out := ikInspectRunResult{Threads: make([][]ikInspectCellResult, len(result.Threads))} + for threadIdx, cells := range result.Threads { + rows := make([]ikInspectCellResult, len(cells)) + for cellIdx, cell := range cells { + row := ikInspectCellResult{ + Cost: cell.Cost, + GoalDist: cell.GoalDist, + Exact: cell.Exact, + Valid: cell.Valid, + CheckPathOK: cell.CheckPathOK, + } + if cell.Inputs != nil { + row.Inputs = linearInputsToStrings(cell.Inputs) + } + if cell.StateError != nil { + row.StateError = cell.StateError.Error() + } + if cell.CheckPathError != nil { + row.CheckPathError = cell.CheckPathError.Error() + } + rows[cellIdx] = row + } + out.Threads[threadIdx] = rows + } + writeJSON(w, out) + } +} + func handlePlanRun(logger logging.Logger) http.HandlerFunc { return func(w http.ResponseWriter, r *http.Request) { file := r.URL.Query().Get("file") @@ -1168,6 +1396,7 @@ func RunServer() error { http.HandleFunc("/", handleIndex(logger)) http.HandleFunc("/detail", handleDetail(logger)) http.HandleFunc("/ik-inspect", handleIKInspect(logger)) + http.HandleFunc("/ik-inspect/run", handleIKInspectRun(logger)) http.HandleFunc("/plan/run", handlePlanRun(logger)) http.HandleFunc("/render-plan", handleRenderPlan(logger)) http.HandleFunc("/render-start", handleRenderStart(logger)) From f19eb357578d9a3e5a3b8287fbc190b3d967f6eb Mon Sep 17 00:00:00 2001 From: Dan Gottlieb Date: Wed, 3 Jun 2026 13:50:13 -0400 Subject: [PATCH 05/12] Public APIs to reuse armplanning IK --- motionplan/armplanning/cBiRRT.go | 6 +++--- motionplan/armplanning/cBiRRT_test.go | 4 ++-- motionplan/armplanning/constraint_test.go | 8 +++---- motionplan/armplanning/context.go | 26 +++++++++++------------ motionplan/armplanning/functional_test.go | 4 ++-- motionplan/armplanning/node.go | 26 +++++++++++------------ motionplan/armplanning/plan_manager.go | 10 ++++----- motionplan/armplanning/plan_test.go | 2 +- motionplan/armplanning/real_test.go | 14 ++++++------ motionplan/armplanning/smart_seed_test.go | 4 ++-- motionplan/armplanning/smooth.go | 12 +++++------ motionplan/armplanning/smooth2_test.go | 4 ++-- motionplan/armplanning/smooth_test.go | 8 +++---- 13 files changed, 64 insertions(+), 64 deletions(-) diff --git a/motionplan/armplanning/cBiRRT.go b/motionplan/armplanning/cBiRRT.go index f83723c4d88..6e1650ddfe2 100644 --- a/motionplan/armplanning/cBiRRT.go +++ b/motionplan/armplanning/cBiRRT.go @@ -33,15 +33,15 @@ var debugConstrainNear = false // It uses the Constrained Bidirctional Rapidly-expanding Random Tree algorithm, Berenson et al 2009 // https://ieeexplore.ieee.org/document/5152399/ type cBiRRTMotionPlanner struct { - pc *planContext - psc *planSegmentContext + pc *PlanContext + psc *PlanSegmentContext logger logging.Logger fastGradDescent *ik.NloptIK } // newCBiRRTMotionPlannerWithSeed creates a cBiRRTMotionPlanner object with a user specified random seed. -func newCBiRRTMotionPlanner(ctx context.Context, pc *planContext, psc *planSegmentContext, logger logging.Logger, +func newCBiRRTMotionPlanner(ctx context.Context, pc *PlanContext, psc *PlanSegmentContext, logger logging.Logger, ) (*cBiRRTMotionPlanner, error) { _, span := trace.StartSpan(ctx, "newCBiRRTMotionPlanner") defer span.End() diff --git a/motionplan/armplanning/cBiRRT_test.go b/motionplan/armplanning/cBiRRT_test.go index 1c5e541b25d..2621733fb7d 100644 --- a/motionplan/armplanning/cBiRRT_test.go +++ b/motionplan/armplanning/cBiRRT_test.go @@ -40,10 +40,10 @@ func TestSimpleLinearMotion(t *testing.T) { Constraints: &motionplan.Constraints{}, } - pc, err := newPlanContext(ctx, logger, request, &PlanMeta{}) + pc, err := NewPlanContext(ctx, logger, request, &PlanMeta{}) test.That(t, err, test.ShouldBeNil) - psc, err := newPlanSegmentContext(ctx, pc, referenceframe.FrameSystemInputs{m.Name(): home7}.ToLinearInputs(), goal) + psc, err := NewPlanSegmentContext(ctx, pc, referenceframe.FrameSystemInputs{m.Name(): home7}.ToLinearInputs(), goal) test.That(t, err, test.ShouldBeNil) mp, err := newCBiRRTMotionPlanner(ctx, pc, psc, logger.Sublogger("cbirrt")) diff --git a/motionplan/armplanning/constraint_test.go b/motionplan/armplanning/constraint_test.go index 92c2c5f68a5..4dcc6b4be9c 100644 --- a/motionplan/armplanning/constraint_test.go +++ b/motionplan/armplanning/constraint_test.go @@ -40,10 +40,10 @@ func TestIKTolerances(t *testing.T) { Constraints: &motionplan.Constraints{}, } - pc, err := newPlanContext(ctx, logger, request, &PlanMeta{}) + pc, err := NewPlanContext(ctx, logger, request, &PlanMeta{}) test.That(t, err, test.ShouldBeNil) - psc, err := newPlanSegmentContext(ctx, pc, seed, goal) + psc, err := NewPlanSegmentContext(ctx, pc, seed, goal) test.That(t, err, test.ShouldBeNil) mp, err := newCBiRRTMotionPlanner(ctx, pc, psc, logger.Sublogger("cbirrt")) @@ -66,10 +66,10 @@ func TestIKTolerances(t *testing.T) { Constraints: &motionplan.Constraints{}, } - pc2, err := newPlanContext(ctx, logger, request2, &PlanMeta{}) + pc2, err := NewPlanContext(ctx, logger, request2, &PlanMeta{}) test.That(t, err, test.ShouldBeNil) - psc2, err := newPlanSegmentContext(ctx, pc2, seed, goal) + psc2, err := NewPlanSegmentContext(ctx, pc2, seed, goal) test.That(t, err, test.ShouldBeNil) mp2, err := newCBiRRTMotionPlanner(ctx, pc2, psc2, logger.Sublogger("cbirrt")) diff --git a/motionplan/armplanning/context.go b/motionplan/armplanning/context.go index 348ee1ed727..d078874398e 100644 --- a/motionplan/armplanning/context.go +++ b/motionplan/armplanning/context.go @@ -16,7 +16,7 @@ import ( "go.viam.com/rdk/referenceframe" ) -type planContext struct { +type PlanContext struct { fs *referenceframe.FrameSystem lis *referenceframe.LinearInputsSchema @@ -39,11 +39,11 @@ type planContext struct { collisionCache *motionplan.CollisionCache } -func newPlanContext(ctx context.Context, logger logging.Logger, request *PlanRequest, meta *PlanMeta) (*planContext, error) { - _, span := trace.StartSpan(ctx, "newPlanContext") +func NewPlanContext(ctx context.Context, logger logging.Logger, request *PlanRequest, meta *PlanMeta) (*PlanContext, error) { + _, span := trace.StartSpan(ctx, "NewPlanContext") defer span.End() meta.CollectSolutionDiagnostics = request.PlannerOptions.CollectSolutionDiagnostics - pc := &planContext{ + pc := &PlanContext{ fs: request.FrameSystem, configurationDistanceFunc: motionplan.GetConfigurationDistanceFunc(request.PlannerOptions.ConfigurationDistanceMetric), planOpts: request.PlannerOptions, @@ -70,7 +70,7 @@ func newPlanContext(ctx context.Context, logger logging.Logger, request *PlanReq return pc, nil } -func (pc *planContext) linearizeFSmetric(metric motionplan.StateFSMetric) ik.CostFunc { +func (pc *PlanContext) linearizeFSmetric(metric motionplan.StateFSMetric) ik.CostFunc { return func(ctx context.Context, linearizedInputs []float64) float64 { conf, err := pc.lis.FloatsToInputs(linearizedInputs) if err != nil { @@ -84,8 +84,8 @@ func (pc *planContext) linearizeFSmetric(metric motionplan.StateFSMetric) ik.Cos } } -type planSegmentContext struct { - pc *planContext +type PlanSegmentContext struct { + pc *PlanContext start *referenceframe.LinearInputs origGoal referenceframe.FrameSystemPoses // goals are defined in frames willy nilly @@ -97,12 +97,12 @@ type planSegmentContext struct { checker *motionplan.ConstraintChecker } -func newPlanSegmentContext(ctx context.Context, pc *planContext, start *referenceframe.LinearInputs, +func NewPlanSegmentContext(ctx context.Context, pc *PlanContext, start *referenceframe.LinearInputs, goal referenceframe.FrameSystemPoses, -) (*planSegmentContext, error) { - _, span := trace.StartSpan(ctx, "newPlanSegmentContext") +) (*PlanSegmentContext, error) { + _, span := trace.StartSpan(ctx, "NewPlanSegmentContext") defer span.End() - psc := &planSegmentContext{ + psc := &PlanSegmentContext{ pc: pc, start: start, origGoal: goal, @@ -161,7 +161,7 @@ func newPlanSegmentContext(ctx context.Context, pc *planContext, start *referenc // checkPath returns an error if the interpolation between `start` and `end` violate a constraint // (e.g: we calculcate there will be a collision). If there is an error and `outPath` is non-nil, // `outPath` will be populated with more detailed information. -func (psc *planSegmentContext) checkPath( +func (psc *PlanSegmentContext) checkPath( ctx context.Context, start, end *referenceframe.LinearInputs, checkFinal bool, outPath *pathFeedback, ) error { ctx, span := trace.StartSpan(ctx, "checkPath") @@ -242,7 +242,7 @@ func translateGoalsToWorldPosition( return alteredGoals, nil } -func (pc *planContext) isFatalCollision(err error) bool { +func (pc *PlanContext) isFatalCollision(err error) bool { s := err.Error() if strings.Contains(s, "obstacle constraint: violation") { hasMovingFrame := false diff --git a/motionplan/armplanning/functional_test.go b/motionplan/armplanning/functional_test.go index ab0d0ba5525..a288ef560ee 100644 --- a/motionplan/armplanning/functional_test.go +++ b/motionplan/armplanning/functional_test.go @@ -298,10 +298,10 @@ func testPlanner(t *testing.T, ctx context.Context, config planConfigConstructor Constraints: cfg.Constraints, } - pc, err := newPlanContext(ctx, logger, request, &PlanMeta{}) + pc, err := NewPlanContext(ctx, logger, request, &PlanMeta{}) test.That(t, err, test.ShouldBeNil) - psc, err := newPlanSegmentContext(ctx, pc, cfg.Start.LinearConfiguration(), cfg.Goal.poses) + psc, err := NewPlanSegmentContext(ctx, pc, cfg.Start.LinearConfiguration(), cfg.Goal.poses) test.That(t, err, test.ShouldBeNil) mp, err := newCBiRRTMotionPlanner(ctx, pc, psc, logger.Sublogger("cbirrt")) diff --git a/motionplan/armplanning/node.go b/motionplan/armplanning/node.go index 4d36ab1eaa1..24076564f0d 100644 --- a/motionplan/armplanning/node.go +++ b/motionplan/armplanning/node.go @@ -120,8 +120,8 @@ func extractPath(startMap, goalMap rrtMap, pair *nodePair, matched bool) []*refe return path } -type solutionSolvingState struct { - psc *planSegmentContext +type SolutionSolvingState struct { + psc *PlanSegmentContext maxSolutions int linearSeeds [][]float64 @@ -147,13 +147,13 @@ type solutionSolvingState struct { logger logging.Logger } -func newSolutionSolvingState(ctx context.Context, psc *planSegmentContext, logger logging.Logger) (*solutionSolvingState, error) { - ctx, span := trace.StartSpan(ctx, "newSolutionSolvingState") +func NewSolutionSolvingState(ctx context.Context, psc *PlanSegmentContext, logger logging.Logger) (*SolutionSolvingState, error) { + ctx, span := trace.StartSpan(ctx, "NewSolutionSolvingState") defer span.End() var err error - sss := &solutionSolvingState{ + sss := &SolutionSolvingState{ psc: psc, solutions: []*node{}, failures: newIkConstraintError(psc.pc.fs, psc.checker), @@ -233,7 +233,7 @@ func newSolutionSolvingState(ctx context.Context, psc *planSegmentContext, logge return sss, nil } -func (sss *solutionSolvingState) computeGoodCost(goal referenceframe.FrameSystemPoses) ([]float64, float64, error) { +func (sss *SolutionSolvingState) computeGoodCost(goal referenceframe.FrameSystemPoses) ([]float64, float64, error) { rawRatios, err := computeJointSensitivities(sss.psc.motionChains, sss.psc.start, sss.psc.pc.fs, sss.psc.pc.planOpts.getGoalMetric(goal), sss.logger) if err != nil { @@ -277,7 +277,7 @@ func (sss *solutionSolvingState) computeGoodCost(goal referenceframe.FrameSystem } // return bool is if we should stop because we're done. -func (sss *solutionSolvingState) process(ctx context.Context, stepSolution *ik.Solution) { +func (sss *SolutionSolvingState) process(ctx context.Context, stepSolution *ik.Solution) { if !sss.processInternal(ctx, stepSolution) { return } @@ -336,7 +336,7 @@ func unwrapRotationalJoints(cfg, start []float64, limits []referenceframe.Limit) } // Returns true if a node was appended. -func (sss *solutionSolvingState) processInternal(ctx context.Context, stepSolution *ik.Solution) bool { +func (sss *SolutionSolvingState) processInternal(ctx context.Context, stepSolution *ik.Solution) bool { ctx, span := trace.StartSpan(ctx, "process") defer span.End() sss.processCalls++ @@ -415,7 +415,7 @@ func (sss *solutionSolvingState) processInternal(ctx context.Context, stepSoluti } // return bool is if we should stop because we're done. -func (sss *solutionSolvingState) shouldStopEarly() bool { +func (sss *SolutionSolvingState) shouldStopEarly() bool { elapsed := time.Since(sss.startTime) if sss.fatal != nil { @@ -514,12 +514,12 @@ func (sss *solutionSolvingState) shouldStopEarly() bool { // // If minScore is positive, if a solution scoring below that amount is found, the solver will // terminate and return that one solution. -func getSolutions(ctx context.Context, psc *planSegmentContext, logger logging.Logger) ([]*node, error) { +func getSolutions(ctx context.Context, psc *PlanSegmentContext, logger logging.Logger) ([]*node, error) { if psc.start.Len() == 0 { return nil, fmt.Errorf("getSolutions start can't be empty") } - solvingState, err := newSolutionSolvingState(ctx, psc, logger) + solvingState, err := NewSolutionSolvingState(ctx, psc, logger) if err != nil { return nil, err } @@ -639,7 +639,7 @@ solutionLoop: return solvingState.solutions, nil } -func (sss *solutionSolvingState) flushFailuresToMeta() { +func (sss *SolutionSolvingState) flushFailuresToMeta() { meta := sss.psc.pc.planMeta if !meta.CollectSolutionDiagnostics { return @@ -667,7 +667,7 @@ func neutralBias(limits []referenceframe.Limit, configuration []float64) float64 return bias } -func (sss *solutionSolvingState) debugSeedInfoForWinner(winner *referenceframe.LinearInputs, solveMeta []ik.SeedSolveMetaData) error { +func (sss *SolutionSolvingState) debugSeedInfoForWinner(winner *referenceframe.LinearInputs, solveMeta []ik.SeedSolveMetaData) error { if sss.logger.GetLevel() != logging.DEBUG { return nil } diff --git a/motionplan/armplanning/plan_manager.go b/motionplan/armplanning/plan_manager.go index ccd08fd4f0b..5904f666905 100644 --- a/motionplan/armplanning/plan_manager.go +++ b/motionplan/armplanning/plan_manager.go @@ -15,13 +15,13 @@ import ( // planManager is intended to be the single entry point to motion planners. type planManager struct { - pc *planContext + pc *PlanContext request *PlanRequest logger logging.Logger } func newPlanManager(ctx context.Context, logger logging.Logger, request *PlanRequest, meta *PlanMeta) (*planManager, error) { - pc, err := newPlanContext(ctx, logger, request, meta) + pc, err := NewPlanContext(ctx, logger, request, meta) if err != nil { return nil, err } @@ -137,7 +137,7 @@ func (pm *planManager) planToDirectJoints( return nil, err } - psc, err := newPlanSegmentContext(ctx, pm.pc, start, goalPoses) + psc, err := NewPlanSegmentContext(ctx, pm.pc, start, goalPoses) if err != nil { return nil, err } @@ -189,7 +189,7 @@ func (pm *planManager) planSingleGoal( pm.logger.Debug("start configuration", logging.FloatArrayFormat{"", start.GetLinearizedInputs()}) pm.logger.Debug("going to", goal) - psc, err := newPlanSegmentContext(ctx, pm.pc, start, goal) + psc, err := NewPlanSegmentContext(ctx, pm.pc, start, goal) if err != nil { return nil, err } @@ -308,7 +308,7 @@ type rrtMaps struct { // initRRTsolutions will create the maps to be used by a RRT-based algorithm. It will generate IK // solutions to pre-populate the goal map, and will check if any of those goals are able to be // directly interpolated to. -func initRRTSolutions(ctx context.Context, psc *planSegmentContext, logger logging.Logger) (*rrtSolution, error) { +func initRRTSolutions(ctx context.Context, psc *PlanSegmentContext, logger logging.Logger) (*rrtSolution, error) { ctx, span := trace.StartSpan(ctx, "initRRTSolutions") defer span.End() rrt := &rrtSolution{ diff --git a/motionplan/armplanning/plan_test.go b/motionplan/armplanning/plan_test.go index 6234a5fd3ba..824a006d7b5 100644 --- a/motionplan/armplanning/plan_test.go +++ b/motionplan/armplanning/plan_test.go @@ -306,7 +306,7 @@ func BenchmarkLinearizeFSMetric(b *testing.B) { err = fs.AddFrame(armModel, fs.World()) test.That(b, err, test.ShouldBeNil) - pc, err := newPlanContext(ctx, logger, + pc, err := NewPlanContext(ctx, logger, &PlanRequest{ FrameSystem: fs, PlannerOptions: &PlannerOptions{}, diff --git a/motionplan/armplanning/real_test.go b/motionplan/armplanning/real_test.go index 7ae9cd03029..5e3c5f11f9f 100644 --- a/motionplan/armplanning/real_test.go +++ b/motionplan/armplanning/real_test.go @@ -261,10 +261,10 @@ func TestSandingLargeMove1(t *testing.T) { } } - pc, err := newPlanContext(ctx, logger, req, &PlanMeta{}) + pc, err := NewPlanContext(ctx, logger, req, &PlanMeta{}) test.That(t, err, test.ShouldBeNil) - psc, err := newPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].poses) + psc, err := NewPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].poses) test.That(t, err, test.ShouldBeNil) solution, err := initRRTSolutions(context.Background(), psc, logger.Sublogger("solve")) @@ -471,10 +471,10 @@ func TestSandingWallCollision(t *testing.T) { t.Run("check collision checks pass with smaller resolution", func(t *testing.T) { // Create plan context to validate the path - pc, err := newPlanContext(ctx, logger, req, &PlanMeta{}) + pc, err := NewPlanContext(ctx, logger, req, &PlanMeta{}) test.That(t, err, test.ShouldBeNil) - psc, err := newPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses()) + psc, err := NewPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses()) test.That(t, err, test.ShouldBeNil) trajectory := plan.Trajectory() @@ -540,13 +540,13 @@ func TestWineBadBottleMoveGoodCost(t *testing.T) { req, err := ReadRequestFromFile("data/wine-bad-bottle-move.json") test.That(t, err, test.ShouldBeNil) - pc, err := newPlanContext(ctx, logger, req, &PlanMeta{}) + pc, err := NewPlanContext(ctx, logger, req, &PlanMeta{}) test.That(t, err, test.ShouldBeNil) - psc, err := newPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses()) + psc, err := NewPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses()) test.That(t, err, test.ShouldBeNil) - sss, err := newSolutionSolvingState(ctx, psc, logger) + sss, err := NewSolutionSolvingState(ctx, psc, logger) test.That(t, err, test.ShouldBeNil) logger.Infof("goodCost: %0.4f", sss.goodCost) diff --git a/motionplan/armplanning/smart_seed_test.go b/motionplan/armplanning/smart_seed_test.go index 38e243aba7a..f6834ae6439 100644 --- a/motionplan/armplanning/smart_seed_test.go +++ b/motionplan/armplanning/smart_seed_test.go @@ -328,10 +328,10 @@ func TestSmartSeedPallette2(t *testing.T) { req, err := ReadRequestFromFile("data/pallette2.json") test.That(t, err, test.ShouldBeNil) - pc, err := newPlanContext(ctx, logger, req, &PlanMeta{}) + pc, err := NewPlanContext(ctx, logger, req, &PlanMeta{}) test.That(t, err, test.ShouldBeNil) - psc, err := newPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses()) + psc, err := NewPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses()) test.That(t, err, test.ShouldBeNil) // Resolve which frame is actually moving toward the goal, and where the goal diff --git a/motionplan/armplanning/smooth.go b/motionplan/armplanning/smooth.go index 52218d03447..b6a6f155067 100644 --- a/motionplan/armplanning/smooth.go +++ b/motionplan/armplanning/smooth.go @@ -11,7 +11,7 @@ import ( "go.viam.com/rdk/referenceframe" ) -func simpleSmoothStep(ctx context.Context, psc *planSegmentContext, steps []*referenceframe.LinearInputs, step int, +func simpleSmoothStep(ctx context.Context, psc *PlanSegmentContext, steps []*referenceframe.LinearInputs, step int, ) []*referenceframe.LinearInputs { ctx, span := trace.StartSpan(ctx, "simpleSmoothStep") defer span.End() @@ -30,7 +30,7 @@ func simpleSmoothStep(ctx context.Context, psc *planSegmentContext, steps []*ref // smoothPath will pick two points at random along the path and attempt to do a fast gradient descent directly between // them, which will cut off randomly-chosen points with odd joint angles into something that is a more intuitive motion. -func smoothPathSimple(ctx context.Context, psc *planSegmentContext, +func smoothPathSimple(ctx context.Context, psc *PlanSegmentContext, steps []*referenceframe.LinearInputs, ) []*referenceframe.LinearInputs { ctx, span := trace.StartSpan(ctx, "smoothPathSimple") @@ -51,7 +51,7 @@ func smoothPathSimple(ctx context.Context, psc *planSegmentContext, } func smoothPath( - ctx context.Context, psc *planSegmentContext, steps []*referenceframe.LinearInputs, + ctx context.Context, psc *PlanSegmentContext, steps []*referenceframe.LinearInputs, ) ([]*referenceframe.LinearInputs, error) { ctx, span := trace.StartSpan(ctx, "smoothPlan") defer span.End() @@ -70,7 +70,7 @@ func smoothPath( // where the path comes within twice the minimum distance of an obstacle. // This prevents the smoothed path from getting too close to obstacles during interpolation. func addCloseObstacleWaypoints( - ctx context.Context, psc *planSegmentContext, steps []*referenceframe.LinearInputs, + ctx context.Context, psc *PlanSegmentContext, steps []*referenceframe.LinearInputs, ) ([]*referenceframe.LinearInputs, error) { ctx, span := trace.StartSpan(ctx, "addCloseObstacleWaypoints") defer span.End() @@ -108,7 +108,7 @@ func addCloseObstacleWaypoints( // in each zone. func findCloseObstacleWaypoints( ctx context.Context, - psc *planSegmentContext, + psc *PlanSegmentContext, start, end *referenceframe.LinearInputs, ) ([]*referenceframe.LinearInputs, error) { segment := &motionplan.SegmentFS{ @@ -147,7 +147,7 @@ func findCloseObstacleWaypoints( return closeWaypoints, nil } -func tryOnlyMovingComponentsThatNeedToMove(ctx context.Context, psc *planSegmentContext, +func tryOnlyMovingComponentsThatNeedToMove(ctx context.Context, psc *PlanSegmentContext, steps []*referenceframe.LinearInputs, ) []*referenceframe.LinearInputs { moving, _ := psc.motionChains.framesFilteredByMovingAndNonmoving() diff --git a/motionplan/armplanning/smooth2_test.go b/motionplan/armplanning/smooth2_test.go index b1c3ed58cc3..51593ad1e83 100644 --- a/motionplan/armplanning/smooth2_test.go +++ b/motionplan/armplanning/smooth2_test.go @@ -42,10 +42,10 @@ func TestWineCrazyTouch3(t *testing.T) { // --- - pc, err := newPlanContext(ctx, logger, req, meta) + pc, err := NewPlanContext(ctx, logger, req, meta) test.That(t, err, test.ShouldBeNil) - psc, err := newPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses()) + psc, err := NewPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses()) test.That(t, err, test.ShouldBeNil) // Convert trajectory to steps diff --git a/motionplan/armplanning/smooth_test.go b/motionplan/armplanning/smooth_test.go index 9625989beb8..20883e404cc 100644 --- a/motionplan/armplanning/smooth_test.go +++ b/motionplan/armplanning/smooth_test.go @@ -45,10 +45,10 @@ func TestSmoothPlans1(t *testing.T) { test.That(t, err, test.ShouldBeNil) req.myTestOptions.doNotCloseObstacles = true - pc, err := newPlanContext(ctx, logger, req, &PlanMeta{}) + pc, err := NewPlanContext(ctx, logger, req, &PlanMeta{}) test.That(t, err, test.ShouldBeNil) - psc, err := newPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses()) + psc, err := NewPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses()) test.That(t, err, test.ShouldBeNil) // Convert []*node to []referenceframe.FrameSystemInputs for smoothPath @@ -80,10 +80,10 @@ func BenchmarkSmoothPlans1(b *testing.B) { req.myTestOptions.doNotCloseObstacles = true test.That(b, err, test.ShouldBeNil) - pc, err := newPlanContext(ctx, logger, req, &PlanMeta{}) + pc, err := NewPlanContext(ctx, logger, req, &PlanMeta{}) test.That(b, err, test.ShouldBeNil) - psc, err := newPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses()) + psc, err := NewPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses()) test.That(b, err, test.ShouldBeNil) // Convert []*node to []referenceframe.FrameSystemInputs for smoothPath From 1bb4901fdbb27e348e898ccb107804b2dda009b9 Mon Sep 17 00:00:00 2001 From: Dan Gottlieb Date: Wed, 3 Jun 2026 14:01:15 -0400 Subject: [PATCH 06/12] Pass around start state explicitly --- motionplan/armplanning/mpserver/ik.go | 19 ++++++++++- motionplan/armplanning/mpserver/server.go | 39 +++++++++++++++++------ 2 files changed, 48 insertions(+), 10 deletions(-) diff --git a/motionplan/armplanning/mpserver/ik.go b/motionplan/armplanning/mpserver/ik.go index 3fb9cde14cb..2fedbc518f7 100644 --- a/motionplan/armplanning/mpserver/ik.go +++ b/motionplan/armplanning/mpserver/ik.go @@ -5,6 +5,7 @@ import ( "errors" "go.viam.com/rdk/logging" + "go.viam.com/rdk/motionplan/armplanning" "go.viam.com/rdk/referenceframe" ) @@ -37,6 +38,22 @@ type IKInspectTable struct { Threads [][]IKInspectCell } -func InspectIK(ctx context.Context, logger logging.Logger, fs *referenceframe.FrameSystem, goal referenceframe.FrameSystemPoses, numSolutions int) (*IKInspectTable, error) { +func InspectIK(ctx context.Context, logger logging.Logger, + req *armplanning.PlanRequest, + segmentStart referenceframe.FrameSystemInputs, + segmentGoal referenceframe.FrameSystemPoses, + numSolutions int, +) (*IKInspectTable, error) { + // var meta armplanning.PlanMeta + // pc, err := armplanning.NewPlanContext(ctx, logger, req, &meta) + // if err != nil { + // return nil, err + // } + // + // psc, err := armplanning.NewPlanSegmentContext(ctx, pc, segmentStart, segmentGoal) + // if err != nil { + // return nil, err + // } + return nil, errors.New("unimplemented") } diff --git a/motionplan/armplanning/mpserver/server.go b/motionplan/armplanning/mpserver/server.go index 3dbd96918ee..60827eda108 100644 --- a/motionplan/armplanning/mpserver/server.go +++ b/motionplan/armplanning/mpserver/server.go @@ -435,6 +435,7 @@ var ikInspectTmpl = template.Must(template.New("ik-inspect").Parse(` +const START_CONFIG = {{.StartConfigJSON}}; const GOAL_POSES = {{.GoalPosesJSON}}; function renderStartAndGoals() { @@ -451,6 +452,7 @@ function runIKInspect() { const div = document.getElementById('ik-result'); div.textContent = 'Running IK inspection…'; fetch('/ik-inspect/run?file=' + encodeURIComponent('{{.File}}') + + '&start_config=' + encodeURIComponent(JSON.stringify(START_CONFIG)) + '&goal_poses=' + encodeURIComponent(JSON.stringify(GOAL_POSES)), { signal: ikInspectAbort.signal }) .then(r => r.json()) @@ -550,10 +552,11 @@ type detailData struct { } type ikInspectData struct { - File string - StartConfig []frameInputs - GoalPoses []poseDisplay - GoalPosesJSON template.JS + File string + StartConfig []frameInputs + StartConfigJSON template.JS + GoalPoses []poseDisplay + GoalPosesJSON template.JS } type planRunResult struct { @@ -997,12 +1000,14 @@ func handleIKInspect(logger logging.Logger) http.HandlerFunc { }) } sort.Slice(startConfig, func(i, j int) bool { return startConfig[i].Name < startConfig[j].Name }) + startConfigJSONBytes, _ := json.Marshal(startConfigStrings) goalPosesJSONBytes, _ := json.Marshal(goalPoseMap) data := ikInspectData{ - File: file, - StartConfig: startConfig, - GoalPoses: poseMapToDisplays(goalPoseMap), - GoalPosesJSON: template.JS(goalPosesJSONBytes), //nolint:gosec + File: file, + StartConfig: startConfig, + StartConfigJSON: template.JS(startConfigJSONBytes), //nolint:gosec + GoalPoses: poseMapToDisplays(goalPoseMap), + GoalPosesJSON: template.JS(goalPosesJSONBytes), //nolint:gosec } w.Header().Set("Content-Type", "text/html; charset=utf-8") if err := ikInspectTmpl.Execute(w, data); err != nil { @@ -1018,6 +1023,22 @@ func handleIKInspectRun(logger logging.Logger) http.HandlerFunc { writeJSON(w, ikInspectRunResult{Error: "missing file parameter"}) return } + var startConfigStrings map[string][]string + if sc := r.URL.Query().Get("start_config"); sc != "" { + if err := json.Unmarshal([]byte(sc), &startConfigStrings); err != nil { + writeJSON(w, ikInspectRunResult{Error: fmt.Sprintf("parsing start_config: %v", err)}) + return + } + } + if len(startConfigStrings) == 0 { + writeJSON(w, ikInspectRunResult{Error: "start_config is required"}) + return + } + segmentStart, err := stringsToLinearInputs(startConfigStrings) + if err != nil { + writeJSON(w, ikInspectRunResult{Error: fmt.Sprintf("parsing start_config values: %v", err)}) + return + } var goalPoseMap map[string]poseComponents if gp := r.URL.Query().Get("goal_poses"); gp != "" { if err := json.Unmarshal([]byte(gp), &goalPoseMap); err != nil { @@ -1046,7 +1067,7 @@ func handleIKInspectRun(logger logging.Logger) http.HandlerFunc { } armplanning.ClearSeedCache() - result, err := InspectIK(r.Context(), logger.Sublogger("ik-inspect"), req.FrameSystem, goalPoses, 10) + result, err := InspectIK(r.Context(), logger.Sublogger("ik-inspect"), req, segmentStart.ToFrameSystemInputs(), goalPoses, 10) if err != nil { writeJSON(w, ikInspectRunResult{Error: err.Error()}) return From fc99d6b7adc92795f7ff8380b89c07ffa1e0a2f5 Mon Sep 17 00:00:00 2001 From: Dan Gottlieb Date: Wed, 3 Jun 2026 16:02:17 -0400 Subject: [PATCH 07/12] More public things. Recreate IK solving with these. --- motionplan/armplanning/cBiRRT.go | 2 +- motionplan/armplanning/context.go | 6 +- motionplan/armplanning/mpserver/ik.go | 91 ++++++++++++++++++----- motionplan/armplanning/mpserver/server.go | 5 +- motionplan/armplanning/node.go | 52 ++++++------- motionplan/armplanning/plan_test.go | 4 +- motionplan/armplanning/planner_options.go | 4 +- referenceframe/linear_inputs.go | 16 +++- 8 files changed, 126 insertions(+), 54 deletions(-) diff --git a/motionplan/armplanning/cBiRRT.go b/motionplan/armplanning/cBiRRT.go index 6e1650ddfe2..4c80290fbb2 100644 --- a/motionplan/armplanning/cBiRRT.go +++ b/motionplan/armplanning/cBiRRT.go @@ -320,7 +320,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear( linearSeed := target.GetLinearizedInputs() var totalAttempts atomic.Int32 solutions, _, err := ik.DoSolve(ctx, mp.fastGradDescent, &totalAttempts, - mp.psc.pc.linearizeFSmetric(myFunc), + mp.psc.pc.LinearizeFSMetric(myFunc), [][]float64{linearSeed}, [][]referenceframe.Limit{ik.ComputeAdjustLimits(linearSeed, mp.pc.lis.GetLimits(), .05)}) if err != nil { mp.logger.Debugf("constrainNear fail (DoSolve): %v", err) diff --git a/motionplan/armplanning/context.go b/motionplan/armplanning/context.go index d078874398e..0aca5e123bb 100644 --- a/motionplan/armplanning/context.go +++ b/motionplan/armplanning/context.go @@ -70,7 +70,11 @@ func NewPlanContext(ctx context.Context, logger logging.Logger, request *PlanReq return pc, nil } -func (pc *PlanContext) linearizeFSmetric(metric motionplan.StateFSMetric) ik.CostFunc { +func (pc *PlanContext) GetLinearInputsSchema() *referenceframe.LinearInputsSchema { + return pc.lis +} + +func (pc *PlanContext) LinearizeFSMetric(metric motionplan.StateFSMetric) ik.CostFunc { return func(ctx context.Context, linearizedInputs []float64) float64 { conf, err := pc.lis.FloatsToInputs(linearizedInputs) if err != nil { diff --git a/motionplan/armplanning/mpserver/ik.go b/motionplan/armplanning/mpserver/ik.go index 2fedbc518f7..623cb890857 100644 --- a/motionplan/armplanning/mpserver/ik.go +++ b/motionplan/armplanning/mpserver/ik.go @@ -2,21 +2,20 @@ package mpserver import ( "context" - "errors" + "math/rand" + "time" "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan/armplanning" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" ) // IKInspectCell describes a single IK solution emitted by one nlopt thread, scored and validated the // same way getSolutions would score and validate it. type IKInspectCell struct { - // Cost is the configuration-distance cost of moving from the start configuration to this - // solution — the same "score" reported in the per-goal solution tables. + // Cost is IK score, but without any "neutral bias". Cost float64 - // GoalDist is the IK solver's own residual: how close the solution's pose is to the goal pose. - GoalDist float64 // Exact is true when the solver considered the goal met (GoalDist below the goal threshold). Exact bool // Inputs is the solution configuration. @@ -35,7 +34,7 @@ type IKInspectCell struct { } type IKInspectTable struct { - Threads [][]IKInspectCell + Rows [][]IKInspectCell } func InspectIK(ctx context.Context, logger logging.Logger, @@ -44,16 +43,72 @@ func InspectIK(ctx context.Context, logger logging.Logger, segmentGoal referenceframe.FrameSystemPoses, numSolutions int, ) (*IKInspectTable, error) { - // var meta armplanning.PlanMeta - // pc, err := armplanning.NewPlanContext(ctx, logger, req, &meta) - // if err != nil { - // return nil, err - // } - // - // psc, err := armplanning.NewPlanSegmentContext(ctx, pc, segmentStart, segmentGoal) - // if err != nil { - // return nil, err - // } - - return nil, errors.New("unimplemented") + var meta armplanning.PlanMeta + pc, err := armplanning.NewPlanContext(ctx, logger, req, &meta) + if err != nil { + return nil, err + } + + startLinear, err := pc.GetLinearInputsSchema().GetLinearInputs(segmentStart) + if err != nil { + return nil, err + } + + psc, err := armplanning.NewPlanSegmentContext(ctx, pc, startLinear, segmentGoal) + if err != nil { + return nil, err + } + + solver, err := ik.CreateNloptSolver(logger, -1, true, true, time.Minute) + if err != nil { + return nil, err + } + + randSeed := rand.New(rand.NewSource(int64(req.PlannerOptions.RandomSeed))) + minimizingFunc := pc.LinearizeFSMetric(req.PlannerOptions.GetGoalMetric(segmentGoal)) + retChan := make(chan *ik.Solution, 10) + + sss, err := armplanning.NewSolutionSolvingState(ctx, psc, logger) + if err != nil { + return nil, err + } + + var seeds [][]float64 = sss.LinearSeeds + var limits [][]referenceframe.Limit = sss.SeedLimits + + ctxWithCancel, cancel := context.WithCancel(ctx) + var solveErr error + go func() { + _, _, solveErr = solver.Solve(ctxWithCancel, retChan, nil, + seeds, limits, minimizingFunc, randSeed.Int()) + cancel() + }() + + var ret IKInspectTable + rowIdx := len(ret.Rows) + ret.Rows = append(ret.Rows, make([]IKInspectCell, 0, 10)) + for len(ret.Rows[rowIdx]) < 10 { + select { + case <-ctxWithCancel.Done(): + // Solver error + return nil, solveErr + case solution := <-retChan: + cells := &ret.Rows[rowIdx] + inputs, err := pc.GetLinearInputsSchema().FloatsToInputs(solution.Configuration) + if err != nil { + return nil, err + } + + *cells = append(*cells, IKInspectCell{ + Cost: solution.Score, + Exact: solution.Exact, + Inputs: inputs, + Valid: true, + CheckPathOK: true, + }) + } + } + cancel() + + return &ret, nil } diff --git a/motionplan/armplanning/mpserver/server.go b/motionplan/armplanning/mpserver/server.go index 60827eda108..b1a07c459b8 100644 --- a/motionplan/armplanning/mpserver/server.go +++ b/motionplan/armplanning/mpserver/server.go @@ -1073,13 +1073,12 @@ func handleIKInspectRun(logger logging.Logger) http.HandlerFunc { return } - out := ikInspectRunResult{Threads: make([][]ikInspectCellResult, len(result.Threads))} - for threadIdx, cells := range result.Threads { + out := ikInspectRunResult{Threads: make([][]ikInspectCellResult, len(result.Rows))} + for threadIdx, cells := range result.Rows { rows := make([]ikInspectCellResult, len(cells)) for cellIdx, cell := range cells { row := ikInspectCellResult{ Cost: cell.Cost, - GoalDist: cell.GoalDist, Exact: cell.Exact, Valid: cell.Valid, CheckPathOK: cell.CheckPathOK, diff --git a/motionplan/armplanning/node.go b/motionplan/armplanning/node.go index 24076564f0d..ead11483041 100644 --- a/motionplan/armplanning/node.go +++ b/motionplan/armplanning/node.go @@ -124,8 +124,8 @@ type SolutionSolvingState struct { psc *PlanSegmentContext maxSolutions int - linearSeeds [][]float64 - seedLimits [][]referenceframe.Limit + LinearSeeds [][]float64 + SeedLimits [][]referenceframe.Limit moving, nonmoving []string @@ -168,8 +168,8 @@ func NewSolutionSolvingState(ctx context.Context, psc *PlanSegmentContext, logge sss.maxSolutions = defaultSolutionsToSeed } - sss.linearSeeds = [][]float64{psc.start.GetLinearizedInputs()} // s:0 - sss.seedLimits = [][]referenceframe.Limit{psc.pc.lis.GetLimits()} + sss.LinearSeeds = [][]float64{psc.start.GetLinearizedInputs()} // s:0 + sss.SeedLimits = [][]referenceframe.Limit{psc.pc.lis.GetLimits()} // For multi-arm systems, `rawRatios` elements will be -1 for non-moving arms. `minRatio` will // be the smallest (currently bumped to a 0.03 minimum) ratio value for joints in moving arms. @@ -178,18 +178,18 @@ func NewSolutionSolvingState(ctx context.Context, psc *PlanSegmentContext, logge return nil, err } - sss.linearSeeds = append(sss.linearSeeds, sss.linearSeeds[0]) // s:1 - sss.seedLimits = append(sss.seedLimits, - ik.ComputeAdjustLimitsArray(sss.linearSeeds[0], sss.seedLimits[0], clampSensitivities(rawRatios, .03))) + sss.LinearSeeds = append(sss.LinearSeeds, sss.LinearSeeds[0]) // s:1 + sss.SeedLimits = append(sss.SeedLimits, + ik.ComputeAdjustLimitsArray(sss.LinearSeeds[0], sss.SeedLimits[0], clampSensitivities(rawRatios, .03))) - sss.linearSeeds = append(sss.linearSeeds, sss.linearSeeds[0]) // s:2 - sss.seedLimits = append(sss.seedLimits, - ik.ComputeAdjustLimitsArray(sss.linearSeeds[0], sss.seedLimits[0], clampSensitivities(rawRatios, .25))) + sss.LinearSeeds = append(sss.LinearSeeds, sss.LinearSeeds[0]) // s:2 + sss.SeedLimits = append(sss.SeedLimits, + ik.ComputeAdjustLimitsArray(sss.LinearSeeds[0], sss.SeedLimits[0], clampSensitivities(rawRatios, .25))) if len(rawRatios) > 6 { // for multi-arms, add a seed that moves just the moving arms with complete freedom - sss.linearSeeds = append(sss.linearSeeds, sss.linearSeeds[0]) - sss.seedLimits = append(sss. - seedLimits, ik.ComputeAdjustLimitsArray(sss.linearSeeds[0], sss.seedLimits[0], clampSensitivities(rawRatios, 1))) + sss.LinearSeeds = append(sss.LinearSeeds, sss.LinearSeeds[0]) + sss.SeedLimits = append(sss. + SeedLimits, ik.ComputeAdjustLimitsArray(sss.LinearSeeds[0], sss.SeedLimits[0], clampSensitivities(rawRatios, 1))) } if sss.goodCost > 1 && minRatio > .05 { @@ -211,14 +211,14 @@ func NewSolutionSolvingState(ctx context.Context, psc *PlanSegmentContext, logge logger.Debugf("\t altLimitDivisors %v", logging.FloatArrayFormat{"", altLimitDivisors}) for _, s := range altSeeds { si := s.GetLinearizedInputs() - sss.linearSeeds = append(sss.linearSeeds, si) - ll := ik.ComputeAdjustLimitsArray(si, sss.seedLimits[0], altLimitDivisors) - sss.seedLimits = append(sss.seedLimits, ll) - logger.Debugf("\t ss (%d): %v", len(sss.linearSeeds)-1, logging.FloatArrayFormat{"", si}) + sss.LinearSeeds = append(sss.LinearSeeds, si) + ll := ik.ComputeAdjustLimitsArray(si, sss.SeedLimits[0], altLimitDivisors) + sss.SeedLimits = append(sss.SeedLimits, ll) + logger.Debugf("\t ss (%d): %v", len(sss.LinearSeeds)-1, logging.FloatArrayFormat{"", si}) } } else { - sss.linearSeeds = append(sss.linearSeeds, sss.linearSeeds[0]) - sss.seedLimits = append(sss.seedLimits, ik.ComputeAdjustLimits(sss.linearSeeds[0], sss.seedLimits[0], .05)) + sss.LinearSeeds = append(sss.LinearSeeds, sss.LinearSeeds[0]) + sss.SeedLimits = append(sss.SeedLimits, ik.ComputeAdjustLimits(sss.LinearSeeds[0], sss.SeedLimits[0], .05)) } sss.moving, sss.nonmoving = sss.psc.motionChains.framesFilteredByMovingAndNonmoving() @@ -235,7 +235,7 @@ func NewSolutionSolvingState(ctx context.Context, psc *PlanSegmentContext, logge func (sss *SolutionSolvingState) computeGoodCost(goal referenceframe.FrameSystemPoses) ([]float64, float64, error) { rawRatios, err := computeJointSensitivities(sss.psc.motionChains, sss.psc.start, sss.psc.pc.fs, - sss.psc.pc.planOpts.getGoalMetric(goal), sss.logger) + sss.psc.pc.planOpts.GetGoalMetric(goal), sss.logger) if err != nil { return nil, 1, err } @@ -251,7 +251,7 @@ func (sss *SolutionSolvingState) computeGoodCost(goal referenceframe.FrameSystem adjusted := []float64{} for idx, r := range ratios { - val := sss.linearSeeds[0][idx] + val := sss.LinearSeeds[0][idx] if r > 0 { // we use min ratio here because we're pretty sure that if we moved every joint minRatio amount // we'd move plenty @@ -525,7 +525,7 @@ func getSolutions(ctx context.Context, psc *PlanSegmentContext, logger logging.L } // Spawn the IK solver to generate solutions until done - minFunc := psc.pc.linearizeFSmetric(psc.pc.planOpts.getGoalMetric(psc.goal)) + minFunc := psc.pc.LinearizeFSMetric(psc.pc.planOpts.GetGoalMetric(psc.goal)) ctxWithCancel, cancel := context.WithCancel(ctx) defer cancel() @@ -568,7 +568,7 @@ func getSolutions(ctx context.Context, psc *PlanSegmentContext, logger logging.L // This channel close doubles as signaling that the goroutine has exited. defer close(solutionGen) nSol, m, err := solver.Solve(ctxWithCancel, solutionGen, &solvingState.totalIkAttempts, - solvingState.linearSeeds, solvingState.seedLimits, minFunc, psc.pc.randseed.Int()) + solvingState.LinearSeeds, solvingState.SeedLimits, minFunc, psc.pc.randseed.Int()) if err == nil { solvingState.logger.Debugf("Solver stopped, no errors. Solutions: %v IK Meta: %v", nSol, m) } else { @@ -694,13 +694,13 @@ func (sss *SolutionSolvingState) debugSeedInfoForWinner(winner *referenceframe.L fmt.Fprintf(&builder, "\t joint %d min: %0.2f, max: %0.2f range: %0.2f\n", jointNumber, min, max, r) fmt.Fprintf(&builder, "\t\t winner: %0.2f\n", winningValue) - for seedNumber, s := range sss.linearSeeds { + for seedNumber, s := range sss.LinearSeeds { step, err := sss.psc.pc.lis.FloatsToInputs(s) if err != nil { return err } v := step.Get(frameName)[jointNumber] - myLimit := sss.seedLimits[seedNumber][jointNumber] + myLimit := sss.SeedLimits[seedNumber][jointNumber] fmt.Fprintf(&builder, "\t\t seed %d %0.2f delta: %0.2f valid: %v limits: %v\n", seedNumber, v, math.Abs(v-winningValue)/r, myLimit.IsValid(winningValue), myLimit) if !myLimit.IsValid(winningValue) { @@ -712,7 +712,7 @@ func (sss *SolutionSolvingState) debugSeedInfoForWinner(winner *referenceframe.L for idx, m := range solveMeta { fmt.Fprintf(&builder, "seed: %d %#v\n", idx, m) - fmt.Fprintf(&builder, "\t %v\n", logging.FloatArrayFormat{"", sss.linearSeeds[idx]}) + fmt.Fprintf(&builder, "\t %v\n", logging.FloatArrayFormat{"", sss.LinearSeeds[idx]}) fmt.Fprintf(&builder, "\t valid: %v\n", !inValid[idx]) } diff --git a/motionplan/armplanning/plan_test.go b/motionplan/armplanning/plan_test.go index 824a006d7b5..7775abbec75 100644 --- a/motionplan/armplanning/plan_test.go +++ b/motionplan/armplanning/plan_test.go @@ -171,7 +171,7 @@ func BenchmarkGoalMetric(b *testing.B) { err = fs.AddFrame(armModel, fs.World()) test.That(b, err, test.ShouldBeNil) - metricFn := options.getGoalMetric(referenceframe.FrameSystemPoses{"xarm6": goalInFrame}) + metricFn := options.GetGoalMetric(referenceframe.FrameSystemPoses{"xarm6": goalInFrame}) test.That(b, err, test.ShouldBeNil) inps := referenceframe.NewLinearInputs() @@ -331,7 +331,7 @@ func BenchmarkLinearizeFSMetric(b *testing.B) { }, )) - minFunc := pc.linearizeFSmetric(func(_ *motionplan.StateFS) float64 { + minFunc := pc.LinearizeFSMetric(func(_ *motionplan.StateFS) float64 { return 0.0 }) diff --git a/motionplan/armplanning/planner_options.go b/motionplan/armplanning/planner_options.go index c0b1ea8d52b..120fdd227e7 100644 --- a/motionplan/armplanning/planner_options.go +++ b/motionplan/armplanning/planner_options.go @@ -157,8 +157,8 @@ func NewPlannerOptionsFromExtra(extra map[string]interface{}) (*PlannerOptions, return opt, nil } -// getGoalMetric creates the distance metric for the solver using the configured options. -func (p *PlannerOptions) getGoalMetric(goals referenceframe.FrameSystemPoses) motionplan.StateFSMetric { +// GetGoalMetric creates the distance metric for the solver using the configured options. +func (p *PlannerOptions) GetGoalMetric(goals referenceframe.FrameSystemPoses) motionplan.StateFSMetric { cartesianScale := 1.0 orientScale := 100.0 diff --git a/referenceframe/linear_inputs.go b/referenceframe/linear_inputs.go index 120c4c9a27d..c2e0e56863a 100644 --- a/referenceframe/linear_inputs.go +++ b/referenceframe/linear_inputs.go @@ -60,6 +60,21 @@ func (li *LinearInputs) GetSchema(fs *FrameSystem) (*LinearInputsSchema, error) return li.schema, nil } +// GetLinearInputs converts a FrameSystemInputs to LinearInputs using this schema. +func (lis *LinearInputsSchema) GetLinearInputs(fsi FrameSystemInputs) (*LinearInputs, error) { + inputs := make([]Input, 0, 0) + for _, meta := range lis.metas { + frameInputs := fsi[meta.frameName] + if len(frameInputs) != meta.dof { + return nil, fmt.Errorf("Frame DoF does not match Schema. Frame: %v DoF: %v Schema: %v", + meta.frameName, len(frameInputs), meta.dof) + } + inputs = append(inputs, frameInputs...) + } + + return &LinearInputs{schema: lis, inputs: inputs}, nil +} + // FloatsToInputs applies the given schema to a new set of linearized floats. This returns an error // if the wrong number of floats are provided. The returned LinearInputs shares the schema pointer // with the original. Callers that need to mutate the schema (e.g. via Put for new frames) must call @@ -134,7 +149,6 @@ type linearInputMeta struct { // LinearInputs is a memory optimized representation of FrameSystemInputs. The type is expected to // only be used by direct consumers of the frame system library. type LinearInputs struct { - // Cache map[string][]Input ? schema *LinearInputsSchema inputs []Input } From ea46264a53e1227c7b7a98c58b1a7b51596184f4 Mon Sep 17 00:00:00 2001 From: Dan Gottlieb Date: Wed, 3 Jun 2026 23:19:57 -0400 Subject: [PATCH 08/12] render IK solutions --- motionplan/armplanning/mpserver/server.go | 25 ++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/motionplan/armplanning/mpserver/server.go b/motionplan/armplanning/mpserver/server.go index b1a07c459b8..3192984865b 100644 --- a/motionplan/armplanning/mpserver/server.go +++ b/motionplan/armplanning/mpserver/server.go @@ -461,13 +461,13 @@ function runIKInspect() { div.innerHTML = '
Error: ' + escHtml(data.error) + '
'; return; } - div.innerHTML = buildIKTable(data.threads || []); + div.innerHTML = buildIKTable('{{.File}}', data.threads || []); }) .catch(err => { if (err.name !== 'AbortError') div.textContent = 'Fetch error: ' + err; }); } // threads is column-major: threads[col] is the ordered list of solutions thread col emitted. -function buildIKTable(threads) { +function buildIKTable(file, threads) { const legend = '

' + 'valid + checkPath ok' + 'valid, checkPath failed' + @@ -486,7 +486,7 @@ function buildIKTable(threads) { for (let c = 0; c < threads.length; c++) { const cell = threads[c][row]; if (!cell) { html += ''; continue; } - html += renderIKCell(cell); + html += renderIKCell(file, cell); } html += ''; } @@ -494,7 +494,7 @@ function buildIKTable(threads) { return html; } -function renderIKCell(cell) { +function renderIKCell(file, cell) { let cls = 'cell-red'; if (cell.valid && cell.check_path_ok) cls = 'cell-green'; else if (cell.valid) cls = 'cell-yellow'; @@ -506,10 +506,25 @@ function renderIKCell(cell) { if (cell.check_path_error) tip.push('checkPath: ' + cell.check_path_error); const goalDist = (typeof cell.goal_dist === 'number') ? cell.goal_dist.toExponential(2) : cell.goal_dist; - const inner = '' + cell.cost.toFixed(4) + '
d=' + goalDist + ''; + let inner = '' + cell.cost.toFixed(4) + '
d=' + goalDist + ''; + if (cell.inputs) { + const inputsArg = JSON.stringify(cell.inputs); + inner += '
'; + } return '' + inner + ''; } +// renderIKSolution draws a single IK cell's configuration in the visualizer, reusing the +// detail page's /render-solution endpoint (inputs are string-valued floats). +function renderIKSolution(file, inputs) { + fetch('/render-solution?file=' + encodeURIComponent(file), { + method: 'POST', + headers: {'Content-Type': 'application/json'}, + body: JSON.stringify(inputs), + }).then(r => { if (!r.ok) r.text().then(msg => console.error('Render error: ' + msg)); }) + .catch(err => console.error('Render error: ' + err)); +} + function escHtml(s) { return String(s).replace(/&/g,'&').replace(//g,'>'); } From d44015d9eb7e0d06a7132efd553b15fedb61bedd Mon Sep 17 00:00:00 2001 From: Dan Gottlieb Date: Thu, 4 Jun 2026 11:26:23 -0400 Subject: [PATCH 09/12] public checkpath/checker --- motionplan/armplanning/cBiRRT.go | 14 +++++++------- motionplan/armplanning/cBiRRT_test.go | 2 +- motionplan/armplanning/context.go | 18 +++++++++--------- motionplan/armplanning/node.go | 26 +++++++++++++------------- motionplan/armplanning/node_test.go | 22 +++++++++++----------- motionplan/armplanning/plan_manager.go | 4 ++-- motionplan/armplanning/real_test.go | 4 ++-- motionplan/armplanning/smooth.go | 6 +++--- 8 files changed, 48 insertions(+), 48 deletions(-) diff --git a/motionplan/armplanning/cBiRRT.go b/motionplan/armplanning/cBiRRT.go index 4c80290fbb2..03e32da040e 100644 --- a/motionplan/armplanning/cBiRRT.go +++ b/motionplan/armplanning/cBiRRT.go @@ -145,7 +145,7 @@ func (mp *cBiRRTMotionPlanner) rrtRunner( map1reached, map2reached := tryExtend(target) - reachedDelta := mp.pc.configurationDistanceFunc( + reachedDelta := mp.pc.ConfigurationDistanceFunc( &motionplan.SegmentFS{ StartConfiguration: map1reached.inputs, EndConfiguration: map2reached.inputs, @@ -161,7 +161,7 @@ func (mp *cBiRRTMotionPlanner) rrtRunner( target = newConfigurationNode(targetConf) map1reached, map2reached = tryExtend(target) - reachedDelta = mp.pc.configurationDistanceFunc(&motionplan.SegmentFS{ + reachedDelta = mp.pc.ConfigurationDistanceFunc(&motionplan.SegmentFS{ StartConfiguration: map1reached.inputs, EndConfiguration: map2reached.inputs, }) @@ -209,7 +209,7 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend( // 4) further iterations change our best node by close-to-zero amounts // 5) we have iterated more than maxExtendIter times for i := 0; i < maxExtendIter; i++ { - configDistMetric := mp.pc.configurationDistanceFunc + configDistMetric := mp.pc.ConfigurationDistanceFunc dist := configDistMetric( &motionplan.SegmentFS{StartConfiguration: near.inputs, EndConfiguration: target.inputs}) oldDist := configDistMetric( @@ -232,7 +232,7 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend( return oldNear } - nearDist := mp.pc.configurationDistanceFunc( + nearDist := mp.pc.ConfigurationDistanceFunc( &motionplan.SegmentFS{StartConfiguration: oldNear.inputs, EndConfiguration: newNear}) if nearDist < math.Pow(mp.pc.planOpts.InputIdentDist, 3) { @@ -281,7 +281,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear( } // Check if the arc of "seedInputs" to "target" is valid - _, err := mp.psc.checker.CheckStateConstraintsAcrossSegmentFS(ctx, newArc, mp.pc.planOpts.Resolution, true) + _, err := mp.psc.Checker.CheckStateConstraintsAcrossSegmentFS(ctx, newArc, mp.pc.planOpts.Resolution, true) if debugConstrainNear { mp.logger.Infof("\t err %v", err) } @@ -342,7 +342,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear( } } - failpos, err := mp.psc.checker.CheckStateConstraintsAcrossSegmentFS( + failpos, err := mp.psc.Checker.CheckStateConstraintsAcrossSegmentFS( ctx, &motionplan.SegmentFS{ StartConfiguration: seedInputs, @@ -364,7 +364,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear( return nil } - dist := mp.pc.configurationDistanceFunc(&motionplan.SegmentFS{ + dist := mp.pc.ConfigurationDistanceFunc(&motionplan.SegmentFS{ StartConfiguration: seedInputs, EndConfiguration: failpos.EndConfiguration, }) diff --git a/motionplan/armplanning/cBiRRT_test.go b/motionplan/armplanning/cBiRRT_test.go index 2621733fb7d..964dc76b770 100644 --- a/motionplan/armplanning/cBiRRT_test.go +++ b/motionplan/armplanning/cBiRRT_test.go @@ -83,7 +83,7 @@ func TestSimpleLinearMotion(t *testing.T) { // extend goalMap towards the point in seedMap goalReached := mp.constrainedExtend(ctx, 1, goalMap, near2, seedReached) - dist := pc.configurationDistanceFunc( + dist := pc.ConfigurationDistanceFunc( &motionplan.SegmentFS{StartConfiguration: seedReached.inputs, EndConfiguration: goalReached.inputs}, ) test.That(t, dist, test.ShouldBeLessThan, pc.planOpts.InputIdentDist) diff --git a/motionplan/armplanning/context.go b/motionplan/armplanning/context.go index 0aca5e123bb..82722b7ba5e 100644 --- a/motionplan/armplanning/context.go +++ b/motionplan/armplanning/context.go @@ -22,7 +22,7 @@ type PlanContext struct { movableFrames []string - configurationDistanceFunc motionplan.SegmentFSMetric + ConfigurationDistanceFunc motionplan.SegmentFSMetric planOpts *PlannerOptions request *PlanRequest @@ -45,7 +45,7 @@ func NewPlanContext(ctx context.Context, logger logging.Logger, request *PlanReq meta.CollectSolutionDiagnostics = request.PlannerOptions.CollectSolutionDiagnostics pc := &PlanContext{ fs: request.FrameSystem, - configurationDistanceFunc: motionplan.GetConfigurationDistanceFunc(request.PlannerOptions.ConfigurationDistanceMetric), + ConfigurationDistanceFunc: motionplan.GetConfigurationDistanceFunc(request.PlannerOptions.ConfigurationDistanceMetric), planOpts: request.PlannerOptions, request: request, randseed: rand.New(rand.NewSource(int64(request.PlannerOptions.RandomSeed))), //nolint:gosec @@ -98,7 +98,7 @@ type PlanSegmentContext struct { startPoses referenceframe.FrameSystemPoses motionChains *motionChains - checker *motionplan.ConstraintChecker + Checker *motionplan.ConstraintChecker } func NewPlanSegmentContext(ctx context.Context, pc *PlanContext, start *referenceframe.LinearInputs, @@ -143,7 +143,7 @@ func NewPlanSegmentContext(ctx context.Context, pc *PlanContext, start *referenc movingRobotGeometries, staticRobotGeometries := psc.motionChains.geometries(pc.fs, frameSystemGeometries) - psc.checker, err = motionplan.NewConstraintChecker( + psc.Checker, err = motionplan.NewConstraintChecker( pc.planOpts.CollisionBufferMM, pc.request.Constraints, psc.startPoses, @@ -162,11 +162,11 @@ func NewPlanSegmentContext(ctx context.Context, pc *PlanContext, start *referenc return psc, nil } -// checkPath returns an error if the interpolation between `start` and `end` violate a constraint +// CheckPath returns an error if the interpolation between `start` and `end` violate a constraint // (e.g: we calculcate there will be a collision). If there is an error and `outPath` is non-nil, // `outPath` will be populated with more detailed information. -func (psc *PlanSegmentContext) checkPath( - ctx context.Context, start, end *referenceframe.LinearInputs, checkFinal bool, outPath *pathFeedback, +func (psc *PlanSegmentContext) CheckPath( + ctx context.Context, start, end *referenceframe.LinearInputs, checkFinal bool, outPath *PathFeedback, ) error { ctx, span := trace.StartSpan(ctx, "checkPath") defer span.End() @@ -185,7 +185,7 @@ func (psc *PlanSegmentContext) checkPath( } } - validSegment, err := psc.checker.CheckStateConstraintsAcrossSegmentFS( + validSegment, err := psc.Checker.CheckStateConstraintsAcrossSegmentFS( ctx, &motionplan.SegmentFS{ StartConfiguration: start, @@ -200,7 +200,7 @@ func (psc *PlanSegmentContext) checkPath( } if err != nil && outPath != nil { - *outPath = pathFeedback{ + *outPath = PathFeedback{ IsObstacleCollision: strings.Contains(err.Error(), motionplan.ObstacleConstraintDescription) || strings.Contains(err.Error(), motionplan.RobotCollisionConstraintDescription), LastGoodInputs: validSegment.EndConfiguration, diff --git a/motionplan/armplanning/node.go b/motionplan/armplanning/node.go index ead11483041..40edd47b7e4 100644 --- a/motionplan/armplanning/node.go +++ b/motionplan/armplanning/node.go @@ -51,12 +51,12 @@ func fixedStepInterpolation(start, target *node, qstep map[string][]float64) *re return newNear } -type pathFeedback struct { +type PathFeedback struct { IsObstacleCollision bool LastGoodInputs *referenceframe.LinearInputs } -func (pf *pathFeedback) String() string { +func (pf *PathFeedback) String() string { return fmt.Sprintf("IsObstacleCollision: %v LastGoodInputs: %v", pf.IsObstacleCollision, pf.LastGoodInputs) } @@ -71,7 +71,7 @@ type node struct { // checkPathError and checkPathFeedback are only captured when // PlanMeta.CollectSolutionDiagnostics is true. checkPathError error - checkPathFeedback pathFeedback + checkPathFeedback PathFeedback } func newConfigurationNode(q *referenceframe.LinearInputs) *node { @@ -156,7 +156,7 @@ func NewSolutionSolvingState(ctx context.Context, psc *PlanSegmentContext, logge sss := &SolutionSolvingState{ psc: psc, solutions: []*node{}, - failures: newIkConstraintError(psc.pc.fs, psc.checker), + failures: newIkConstraintError(psc.pc.fs, psc.Checker), firstSolutionTime: time.Hour, bestScoreNoProblem: 10000000, bestScoreWithProblem: 10000000, @@ -271,7 +271,7 @@ func (sss *SolutionSolvingState) computeGoodCost(goal referenceframe.FrameSystem FS: sss.psc.pc.fs, } - sss.goodCost = sss.psc.pc.configurationDistanceFunc(stepArc) + sss.goodCost = sss.psc.pc.ConfigurationDistanceFunc(stepArc) sss.logger.Debugf("goodCost: %0.2f minRatio: %0.2f", sss.goodCost, minRatio) return rawRatios, minRatio, nil } @@ -352,9 +352,9 @@ func (sss *SolutionSolvingState) processInternal(ctx context.Context, stepSoluti EndConfiguration: step, FS: sss.psc.pc.fs, } - myCost := sss.psc.pc.configurationDistanceFunc(stepArc) + myCost := sss.psc.pc.ConfigurationDistanceFunc(stepArc) - myCost += neutralBias(sss.psc.pc.lis.GetLimits(), stepSolution.Configuration) + myCost += NeutralBias(sss.psc.pc.lis.GetLimits(), stepSolution.Configuration) if myCost > sss.bestScoreNoProblem { sss.logger.Debugf("got score %0.4f worse than bestScoreNoProblem", myCost) @@ -367,14 +367,14 @@ func (sss *SolutionSolvingState) processInternal(ctx context.Context, stepSoluti EndConfiguration: step, FS: sss.psc.pc.fs, } - simscore := sss.psc.pc.configurationDistanceFunc(similarity) + simscore := sss.psc.pc.ConfigurationDistanceFunc(similarity) if simscore < defaultSimScore { return false } } // Ensure the end state is a valid one - _, err = sss.psc.checker.CheckStateFSConstraints(ctx, &motionplan.StateFS{ + _, err = sss.psc.Checker.CheckStateFSConstraints(ctx, &motionplan.StateFS{ Configuration: step, FS: sss.psc.pc.fs, }) @@ -400,9 +400,9 @@ func (sss *SolutionSolvingState) processInternal(ctx context.Context, stepSoluti } if sss.psc.pc.planMeta.CollectSolutionDiagnostics { - myNode.checkPathError = sss.psc.checkPath(ctx, sss.psc.start, step, false, &myNode.checkPathFeedback) + myNode.checkPathError = sss.psc.CheckPath(ctx, sss.psc.start, step, false, &myNode.checkPathFeedback) } else { - myNode.checkPathError = sss.psc.checkPath(ctx, sss.psc.start, step, false, nil) + myNode.checkPathError = sss.psc.CheckPath(ctx, sss.psc.start, step, false, nil) } sss.logger.Debugf("got score %0.4f @ %v - %s - result: %v", myNode.cost, now, stepSolution.Meta, myNode.checkPathError) @@ -651,9 +651,9 @@ func (sss *SolutionSolvingState) flushFailuresToMeta() { } } -// neutralBias computes a small cost penalty for rotational joints that are far from the center of their range. +// NeutralBias computes a small cost penalty for rotational joints that are far from the center of their range. // This favors solutions where rotational joints are near the midpoint rather than at the extremes. -func neutralBias(limits []referenceframe.Limit, configuration []float64) float64 { +func NeutralBias(limits []referenceframe.Limit, configuration []float64) float64 { bias := 0.0 for i, limit := range limits { if limit.IsRotational() { diff --git a/motionplan/armplanning/node_test.go b/motionplan/armplanning/node_test.go index e890e8a5615..ac4f4d13415 100644 --- a/motionplan/armplanning/node_test.go +++ b/motionplan/armplanning/node_test.go @@ -44,19 +44,19 @@ func TestNeutralBias(t *testing.T) { } // At center (0), no bias - biasAtCenter := neutralBias(rotLimits, []float64{0, 0, 0}) + biasAtCenter := NeutralBias(rotLimits, []float64{0, 0, 0}) test.That(t, biasAtCenter, test.ShouldEqual, 0) // At extremes, maximum bias - biasAtPi := neutralBias(rotLimits, []float64{math.Pi, math.Pi, math.Pi}) + biasAtPi := NeutralBias(rotLimits, []float64{math.Pi, math.Pi, math.Pi}) test.That(t, biasAtPi, test.ShouldBeGreaterThan, 0) // Symmetric extremes should produce equal bias - biasAtNegPi := neutralBias(rotLimits, []float64{-math.Pi, -math.Pi, -math.Pi}) + biasAtNegPi := NeutralBias(rotLimits, []float64{-math.Pi, -math.Pi, -math.Pi}) test.That(t, biasAtNegPi, test.ShouldAlmostEqual, biasAtPi) // Closer to center should have less bias - biasSmall := neutralBias(rotLimits, []float64{0.1, 0.1, 0.1}) + biasSmall := NeutralBias(rotLimits, []float64{0.1, 0.1, 0.1}) test.That(t, biasSmall, test.ShouldBeGreaterThan, 0) test.That(t, biasSmall, test.ShouldBeLessThan, biasAtPi) @@ -65,17 +65,17 @@ func TestNeutralBias(t *testing.T) { {Min: -math.Pi, Max: math.Pi}, // rotational {Min: 0, Max: 100}, // linear } - biasNonRot := neutralBias(mixedLimits, []float64{math.Pi, 50}) - biasRotOnly := neutralBias(mixedLimits, []float64{math.Pi, 0}) + biasNonRot := NeutralBias(mixedLimits, []float64{math.Pi, 50}) + biasRotOnly := NeutralBias(mixedLimits, []float64{math.Pi, 0}) test.That(t, biasNonRot, test.ShouldEqual, biasRotOnly) // Asymmetric rotational limits: center is pi/2 asymLimits := []referenceframe.Limit{ {Min: 0, Max: math.Pi}, } - biasAtMid := neutralBias(asymLimits, []float64{math.Pi / 2}) - biasAtMin := neutralBias(asymLimits, []float64{0}) - biasAtMax := neutralBias(asymLimits, []float64{math.Pi}) + biasAtMid := NeutralBias(asymLimits, []float64{math.Pi / 2}) + biasAtMin := NeutralBias(asymLimits, []float64{0}) + biasAtMax := NeutralBias(asymLimits, []float64{math.Pi}) test.That(t, biasAtMid, test.ShouldEqual, 0) test.That(t, biasAtMin, test.ShouldAlmostEqual, biasAtMax) test.That(t, biasAtMin, test.ShouldBeGreaterThan, 0) @@ -85,8 +85,8 @@ func TestNeutralBias(t *testing.T) { {Min: -6.265732014659642, Max: 6.26573201465964}, } xarmMid := (-6.265732014659642 + 6.26573201465964) / 2 - biasXarmCenter := neutralBias(xarmLimits, []float64{xarmMid}) - biasXarmPi := neutralBias(xarmLimits, []float64{math.Pi}) + biasXarmCenter := NeutralBias(xarmLimits, []float64{xarmMid}) + biasXarmPi := NeutralBias(xarmLimits, []float64{math.Pi}) test.That(t, biasXarmCenter, test.ShouldAlmostEqual, 0) test.That(t, biasXarmPi, test.ShouldBeGreaterThan, biasXarmCenter) } diff --git a/motionplan/armplanning/plan_manager.go b/motionplan/armplanning/plan_manager.go index 5904f666905..ef745e33430 100644 --- a/motionplan/armplanning/plan_manager.go +++ b/motionplan/armplanning/plan_manager.go @@ -142,13 +142,13 @@ func (pm *planManager) planToDirectJoints( return nil, err } - err = psc.checkPath(ctx, start, fullConfig, false, nil) + err = psc.CheckPath(ctx, start, fullConfig, false, nil) if err == nil { return []*referenceframe.LinearInputs{fullConfig}, nil } pm.logger.Debugf("want to go to specific joint positions, but path is blocked: %v", err) - _, err = psc.checker.CheckStateFSConstraints(ctx, &motionplan.StateFS{ + _, err = psc.Checker.CheckStateFSConstraints(ctx, &motionplan.StateFS{ Configuration: fullConfig, FS: psc.pc.fs, }) diff --git a/motionplan/armplanning/real_test.go b/motionplan/armplanning/real_test.go index 5e3c5f11f9f..c1ffec1629e 100644 --- a/motionplan/armplanning/real_test.go +++ b/motionplan/armplanning/real_test.go @@ -485,11 +485,11 @@ func TestSandingWallCollision(t *testing.T) { end := trajectory[j+1].ToLinearInputs() // Default resolution passes - err := psc.checkPath(ctx, start, end, false, nil) + err := psc.CheckPath(ctx, start, end, false, nil) test.That(t, err, test.ShouldBeNil) // Small resolution noticed the collision when we had large jumps - _, err = psc.checker.CheckStateConstraintsAcrossSegmentFS( + _, err = psc.Checker.CheckStateConstraintsAcrossSegmentFS( ctx, &motionplan.SegmentFS{ StartConfiguration: start, diff --git a/motionplan/armplanning/smooth.go b/motionplan/armplanning/smooth.go index b6a6f155067..5a435925ed4 100644 --- a/motionplan/armplanning/smooth.go +++ b/motionplan/armplanning/smooth.go @@ -17,7 +17,7 @@ func simpleSmoothStep(ctx context.Context, psc *PlanSegmentContext, steps []*ref defer span.End() // look at each triplet, see if we can remove the middle one for i := step + 1; i < len(steps); i += step { - err := psc.checkPath(ctx, steps[i-step-1], steps[i], false, nil) + err := psc.CheckPath(ctx, steps[i-step-1], steps[i], false, nil) if err != nil { continue } @@ -134,7 +134,7 @@ func findCloseObstacleWaypoints( Configuration: interpolated[i], } - closestObstacle, err := psc.checker.CheckStateFSConstraints(ctx, state) + closestObstacle, err := psc.Checker.CheckStateFSConstraints(ctx, state) if err != nil { return nil, err } @@ -172,7 +172,7 @@ func tryOnlyMovingComponentsThatNeedToMove(ctx context.Context, psc *PlanSegment updated.Put(component, prevInputs) } - err := psc.checkPath(ctx, prev, updated, false, nil) + err := psc.CheckPath(ctx, prev, updated, false, nil) if err == nil { steps[idx] = updated } From f1d1eda9efa8c1312c708f8bf3d54661b51491d1 Mon Sep 17 00:00:00 2001 From: Dan Gottlieb Date: Thu, 4 Jun 2026 11:45:56 -0400 Subject: [PATCH 10/12] IK tables looking good --- motionplan/armplanning/mpserver/ik.go | 94 +++++++++++++--------- motionplan/armplanning/mpserver/server.go | 96 +++++++++++++++-------- motionplan/armplanning/node.go | 13 ++- 3 files changed, 132 insertions(+), 71 deletions(-) diff --git a/motionplan/armplanning/mpserver/ik.go b/motionplan/armplanning/mpserver/ik.go index 623cb890857..00b5cbd4ded 100644 --- a/motionplan/armplanning/mpserver/ik.go +++ b/motionplan/armplanning/mpserver/ik.go @@ -6,12 +6,13 @@ import ( "time" "go.viam.com/rdk/logging" + "go.viam.com/rdk/motionplan" "go.viam.com/rdk/motionplan/armplanning" "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" ) -// IKInspectCell describes a single IK solution emitted by one nlopt thread, scored and validated the +// IKInspectCell describes a single IK solution emitted from one seed, scored and validated the // same way getSolutions would score and validate it. type IKInspectCell struct { // Cost is IK score, but without any "neutral bias". @@ -34,7 +35,8 @@ type IKInspectCell struct { } type IKInspectTable struct { - Rows [][]IKInspectCell + Rows [][]IKInspectCell + SeedLabels []string } func InspectIK(ctx context.Context, logger logging.Logger, @@ -49,7 +51,8 @@ func InspectIK(ctx context.Context, logger logging.Logger, return nil, err } - startLinear, err := pc.GetLinearInputsSchema().GetLinearInputs(segmentStart) + linearSchema := pc.GetLinearInputsSchema() + startLinear, err := linearSchema.GetLinearInputs(segmentStart) if err != nil { return nil, err } @@ -65,7 +68,7 @@ func InspectIK(ctx context.Context, logger logging.Logger, } randSeed := rand.New(rand.NewSource(int64(req.PlannerOptions.RandomSeed))) - minimizingFunc := pc.LinearizeFSMetric(req.PlannerOptions.GetGoalMetric(segmentGoal)) + ikMinimizingFunc := pc.LinearizeFSMetric(req.PlannerOptions.GetGoalMetric(segmentGoal)) retChan := make(chan *ik.Solution, 10) sss, err := armplanning.NewSolutionSolvingState(ctx, psc, logger) @@ -73,42 +76,61 @@ func InspectIK(ctx context.Context, logger logging.Logger, return nil, err } - var seeds [][]float64 = sss.LinearSeeds - var limits [][]referenceframe.Limit = sss.SeedLimits - - ctxWithCancel, cancel := context.WithCancel(ctx) - var solveErr error - go func() { - _, _, solveErr = solver.Solve(ctxWithCancel, retChan, nil, - seeds, limits, minimizingFunc, randSeed.Int()) - cancel() - }() - var ret IKInspectTable - rowIdx := len(ret.Rows) - ret.Rows = append(ret.Rows, make([]IKInspectCell, 0, 10)) - for len(ret.Rows[rowIdx]) < 10 { - select { - case <-ctxWithCancel.Done(): - // Solver error - return nil, solveErr - case solution := <-retChan: - cells := &ret.Rows[rowIdx] - inputs, err := pc.GetLinearInputsSchema().FloatsToInputs(solution.Configuration) - if err != nil { - return nil, err + ret.SeedLabels = sss.SeedDescriptions + for seedIdx, seed := range sss.LinearSeeds { + seeds := [][]float64{seed} + limits := [][]referenceframe.Limit{sss.SeedLimits[seedIdx]} + + ctxWithCancel, cancel := context.WithCancel(ctx) + var solveErr error + go func() { + _, _, solveErr = solver.Solve(ctxWithCancel, retChan, nil, + seeds, limits, ikMinimizingFunc, randSeed.Int()) + cancel() + }() + + rowIdx := len(ret.Rows) + ret.Rows = append(ret.Rows, make([]IKInspectCell, 0, 10)) + for len(ret.Rows[rowIdx]) < 10 { + select { + case <-ctxWithCancel.Done(): + // Solver error + return nil, solveErr + case solution := <-retChan: + cells := &ret.Rows[rowIdx] + inputs, err := linearSchema.FloatsToInputs(solution.Configuration) + if err != nil { + return nil, err + } + + _, finalStateErr := psc.Checker.CheckStateFSConstraints(ctx, &motionplan.StateFS{ + Configuration: inputs, + FS: req.FrameSystem, + }) + + var pathFeedback armplanning.PathFeedback + pathError := psc.CheckPath(ctx, startLinear, inputs, false, &pathFeedback) + + stepArc := &motionplan.SegmentFS{ + StartConfiguration: startLinear, + EndConfiguration: inputs, + FS: req.FrameSystem, + } + *cells = append(*cells, IKInspectCell{ + Cost: pc.ConfigurationDistanceFunc(stepArc) + + armplanning.NeutralBias(linearSchema.GetLimits(), solution.Configuration), + Exact: solution.Exact, + Inputs: inputs, + Valid: finalStateErr == nil, + StateError: finalStateErr, + CheckPathOK: pathError == nil, + CheckPathError: pathError, + }) } - - *cells = append(*cells, IKInspectCell{ - Cost: solution.Score, - Exact: solution.Exact, - Inputs: inputs, - Valid: true, - CheckPathOK: true, - }) } + cancel() } - cancel() return &ret, nil } diff --git a/motionplan/armplanning/mpserver/server.go b/motionplan/armplanning/mpserver/server.go index 3192984865b..54d1a1f8b21 100644 --- a/motionplan/armplanning/mpserver/server.go +++ b/motionplan/armplanning/mpserver/server.go @@ -405,11 +405,6 @@ var ikInspectTmpl = template.Must(template.New("ik-inspect").Parse(`IK Inspect

{{.File}} — ← Back to Detail

- -  - -
-

Start Configuration

{{if .StartConfig}} @@ -434,6 +429,18 @@ var ikInspectTmpl = template.Must(template.New("ik-inspect").Parse(`No goal poses.

{{end}} + +  + +
+ +
FrameInputs
+ + +
+