20260207 consistent velocity prep (#5719)

Author: erh

motionplan/armplanning/api.go

@@ -177,9 +177,9 @@ func PlanFrameMotion(ctx context.Context,
 
 // PlanMeta is meta data about plan generation.
 type PlanMeta struct {
-	Duration       time.Duration
-	Partial        bool
-	GoalsProcessed int
+	Duration     time.Duration
+	Partial      bool
+	PartialError error
 }
 
 // PlanMotion plans a motion from a provided plan request.
@@ -217,18 +217,17 @@ func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanR
 		return nil, meta, err
 	}
 
-	trajAsInps, goalsProcessed, err := sfPlanner.planMultiWaypoint(ctx)
+	trajAsInps, err := sfPlanner.planMultiWaypoint(ctx)
 	if err != nil {
 		if request.PlannerOptions.ReturnPartialPlan {
 			meta.Partial = true
+			meta.PartialError = err
 			logger.Infof("returning partial plan, error: %v", err)
 		} else {
 			return nil, meta, err
 		}
 	}
 
-	meta.GoalsProcessed = goalsProcessed
-
 	t, err := motionplan.NewSimplePlanFromTrajectory(trajAsInps, request.FrameSystem)
 	if err != nil {
 		return nil, meta, err

motionplan/armplanning/cmd-plan/cmd-plan.go

@@ -146,7 +146,7 @@ func realMain() error {
 	trace.SetProvider(ctx, sdktrace.WithResource(otelresource.Empty()))
 	trace.AddExporters(spansExporter)
 
-	plan, _, err := armplanning.PlanMotion(ctx, logger, req)
+	plan, meta, err := armplanning.PlanMotion(ctx, logger, req)
 	if err := trace.Shutdown(ctx); err != nil {
 		logger.Errorw("Got error while shutting down tracing", "err", err)
 	}
@@ -229,11 +229,25 @@ func realMain() error {
 		}
 	}
 
+	if meta.PartialError != nil {
+		mylog.Printf("partial results, error: %v", meta.PartialError)
+	}
+
 	mylog.Printf("planning took %v for %d goals => trajectory length: %d",
 		time.Since(start).Truncate(time.Millisecond), len(req.Goals), len(plan.Trajectory()))
 	mylog.Printf("totalCartesion: %0.4f\n", totalCartesion)
 	mylog.Printf("totalL2: %0.4f\n", totalL2)
 
+	// Print delta statistics if trajectory has more than 5 points
+	if len(plan.Trajectory()) > 5 {
+		stats := armplanning.TrajectoryDeltaStats(plan.Trajectory())
+		mylog.Printf("\nDelta Statistics (trajectory length: %d):", len(plan.Trajectory()))
+		for _, s := range stats {
+			mylog.Printf("  %s:%d: avg=%0.5f stddev=%0.5f outside1=%d outside2=%d (n=%d)",
+				s.Component, s.JointIdx, s.Mean, s.StdDev, s.Outside1, s.Outside2, s.Count)
+		}
+	}
+
 	for i := 0; i < *loop; i++ {
 		err = visualize(req, plan, mylog)
 		if err != nil {

motionplan/armplanning/plan_manager.go

@@ -35,7 +35,7 @@ func newPlanManager(ctx context.Context, logger logging.Logger, request *PlanReq
 // planMultiWaypoint plans a motion through multiple waypoints, using identical constraints for each
 // Any constraints, etc, will be held for the entire motion.
 // return trajector (always, even with error), which goal we got to, error.
-func (pm *planManager) planMultiWaypoint(ctx context.Context) ([]*referenceframe.LinearInputs, int, error) {
+func (pm *planManager) planMultiWaypoint(ctx context.Context) ([]*referenceframe.LinearInputs, error) {
 	ctx, span := trace.StartSpan(ctx, "planMultiWaypoint")
 	defer span.End()
 
@@ -51,17 +51,17 @@ func (pm *planManager) planMultiWaypoint(ctx context.Context) ([]*referenceframe
 	linearTraj := []*referenceframe.LinearInputs{pm.request.StartState.LinearConfiguration()}
 	start, err := pm.request.StartState.ComputePoses(ctx, pm.request.FrameSystem)
 	if err != nil {
-		return nil, 0, err
+		return nil, err
 	}
 
 	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
+			return linearTraj, err // note: here and below, we return traj because of ReturnPartialPlan
 		}
 
 		to, err := g.ComputePoses(ctx, pm.request.FrameSystem)
 		if err != nil {
-			return linearTraj, i, err
+			return linearTraj, err
 		}
 
 		pm.logger.Info("planning step", i, "of", len(pm.request.Goals))
@@ -72,13 +72,13 @@ func (pm *planManager) planMultiWaypoint(ctx context.Context) ([]*referenceframe
 		if len(g.Configuration()) > 0 {
 			newTraj, err := pm.planToDirectJoints(ctx, linearTraj[len(linearTraj)-1], g)
 			if err != nil {
-				return linearTraj, i, err
+				return linearTraj, err
 			}
 			linearTraj = append(linearTraj, newTraj...)
 		} else {
 			subGoals, cbirrtAllowed, err := pm.generateWaypoints(ctx, start, to)
 			if err != nil {
-				return linearTraj, i, err
+				return linearTraj, err
 			}
 
 			if len(subGoals) > 1 {
@@ -95,7 +95,7 @@ func (pm *planManager) planMultiWaypoint(ctx context.Context) ([]*referenceframe
 				newTraj, err := pm.planSingleGoal(ctx, linearTraj[len(linearTraj)-1], sg, cbirrtAllowed)
 				if err != nil {
 					pm.logger.Infof("\t subgoal %d failed after %v with: %v", subGoalIdx, time.Since(singleGoalStart), err)
-					return linearTraj, i, err
+					return linearTraj, err
 				}
 				pm.logger.Infof("\t subgoal %d took %v", subGoalIdx, time.Since(singleGoalStart))
 				linearTraj = append(linearTraj, newTraj...)
@@ -104,7 +104,7 @@ func (pm *planManager) planMultiWaypoint(ctx context.Context) ([]*referenceframe
 		start = to
 	}
 
-	return linearTraj, len(pm.request.Goals), nil
+	return linearTraj, nil
 }
 
 func (pm *planManager) planToDirectJoints(

motionplan/armplanning/smooth.go

@@ -2,7 +2,11 @@ package armplanning
 
 import (
 	"context"
+	"fmt"
 	"math"
+	"sort"
+	"strconv"
+	"strings"
 	"time"
 
 	"go.viam.com/utils/trace"
@@ -11,6 +15,116 @@ import (
 	"go.viam.com/rdk/referenceframe"
 )
 
+// JointDeltaStats holds statistics about delta values for a single joint.
+type JointDeltaStats struct {
+	Component string
+	JointIdx  int
+	Count     int
+	Mean      float64
+	StdDev    float64
+	Outside1  int // count of values more than 1 std dev from mean
+	Outside2  int // count of values more than 2 std dev from mean
+}
+
+// TrajectoryDeltaStats computes delta statistics for each joint across all steps in a trajectory.
+// Returns nil if trajectory has fewer than 2 steps.
+func TrajectoryDeltaStats(trajectory motionplan.Trajectory) []JointDeltaStats {
+	if len(trajectory) < 2 {
+		return nil
+	}
+
+	// Collect deltas for each component:joint
+	// key is "component:jointIdx", value is slice of delta values
+	allDeltas := map[string][]float64{}
+
+	for idx := 1; idx < len(trajectory); idx++ {
+		curr := trajectory[idx]
+		prev := trajectory[idx-1]
+
+		for component, currInputs := range curr {
+			prevInputs, ok := prev[component]
+			if !ok || len(prevInputs) == 0 || len(currInputs) == 0 {
+				continue
+			}
+
+			for i, currVal := range currInputs {
+				if i >= len(prevInputs) {
+					break
+				}
+				key := fmt.Sprintf("%s:%d", component, i)
+				delta := currVal - prevInputs[i]
+				allDeltas[key] = append(allDeltas[key], delta)
+			}
+		}
+	}
+
+	// Sort keys for consistent output
+	keys := make([]string, 0, len(allDeltas))
+	for k := range allDeltas {
+		keys = append(keys, k)
+	}
+	sort.Strings(keys)
+
+	// Compute statistics for each joint
+	results := make([]JointDeltaStats, 0, len(keys))
+	for _, key := range keys {
+		deltas := allDeltas[key]
+		n := len(deltas)
+		if n == 0 {
+			continue
+		}
+
+		// Parse component and joint index from key (format: "component:jointIdx")
+		lastColon := strings.LastIndex(key, ":")
+		component := key[:lastColon]
+		jointIdx, err := strconv.Atoi(key[lastColon+1:])
+		if err != nil {
+			// Should not happen since we create the keys ourselves
+			continue
+		}
+
+		// Calculate mean
+		sum := 0.0
+		for _, d := range deltas {
+			sum += d
+		}
+		mean := sum / float64(n)
+
+		// Calculate standard deviation
+		sumSqDiff := 0.0
+		for _, d := range deltas {
+			diff := d - mean
+			sumSqDiff += diff * diff
+		}
+		stdDev := math.Sqrt(sumSqDiff / float64(n))
+
+		// Count values outside 1 and 2 standard deviations
+		outside1 := 0
+		outside2 := 0
+		for _, d := range deltas {
+			diff := math.Abs(d - mean)
+			if diff > stdDev {
+				outside1++
+			}
+			if diff > 2*stdDev {
+				outside2++
+			}
+		}
+
+		results = append(results, JointDeltaStats{
+			Component: component,
+			JointIdx:  jointIdx,
+			Count:     n,
+			Mean:      mean,
+			StdDev:    stdDev,
+			Outside1:  outside1,
+			Outside2:  outside2,
+		})
+	}
+
+	return results
+}
+
 func simpleSmoothStep(ctx context.Context, psc *planSegmentContext, steps []*referenceframe.LinearInputs, step int,
 ) []*referenceframe.LinearInputs {
 	ctx, span := trace.StartSpan(ctx, "simpleSmoothStep")