[DO NOT MERGE] Fix frame system serialize patch

motionplan/armplanning/motionPlanner.go

@@ -248,6 +248,8 @@ func Replan(
 	currentPlan motionplan.Plan,
 	replanCostFactor float64,
 ) (motionplan.Plan, error) {
+	logger.CDebugf(ctx, "replan start")
+	defer logger.CDebugf(ctx, "replan end")
 	// Make sure request is well formed and not missing vital information
 	if err := request.validatePlanRequest(); err != nil {
 		return nil, err
@@ -260,10 +262,12 @@ func Replan(
 		return nil, err
 	}
 
+	logger.CDebugf(ctx, "planMultiWaypoint start")
 	newPlan, err := sfPlanner.planMultiWaypoint(ctx, currentPlan)
 	if err != nil {
 		return nil, err
 	}
+	logger.CDebugf(ctx, "planMultiWaypoint end")
 
 	if replanCostFactor > 0 && currentPlan != nil {
 		initialPlanCost := currentPlan.Trajectory().EvaluateCost(sfPlanner.scoringFunction)
@@ -512,6 +516,8 @@ func (mp *planner) getSolutions(
 	seed referenceframe.FrameSystemInputs,
 	metric motionplan.StateFSMetric,
 ) ([]node, error) {
+	mp.logger.CDebug(ctx, "getSolutions start")
+	defer mp.logger.CDebug(ctx, "getSolutions end")
 	// Linter doesn't properly handle loop labels
 	nSolutions := mp.planOpts.MaxSolutions
 	if nSolutions == 0 {
@@ -563,10 +569,11 @@ func (mp *planner) getSolutions(
 	failures := map[string]int{}
 	constraintFailCnt := 0
 
-	startTime := time.Now()
-	firstSolutionTime := time.Hour
+	// startTime := time.Now()
+	// firstSolutionTime := time.Hour
 
 	// Solve the IK solver. Loop labels are required because `break` etc in a `select` will break only the `select`.
+	i := -1
 IK:
 	for {
 		select {
@@ -577,6 +584,8 @@ IK:
 
 		select {
 		case stepSolution := <-solutionGen:
+			i++
+			mp.logger.CDebugf(ctx, "getSolutions i: %d", i)
 
 			step, err := mp.lfs.sliceToMap(stepSolution.Configuration)
 			if err != nil {
@@ -605,6 +614,7 @@ IK:
 						solutions = map[float64]referenceframe.FrameSystemInputs{}
 						solutions[score] = step
 						// good solution, stopping early
+						mp.logger.CDebugf(ctx, "getSolutions i: %d, good solution, stopping early, score: %d", i, score)
 						break IK
 					}
 					for _, oldSol := range solutions {
@@ -615,25 +625,27 @@ IK:
 						}
 						simscore := mp.configurationDistanceFunc(similarity)
 						if simscore < defaultSimScore {
+							mp.logger.CDebugf(ctx, "getSolutions i: %d, continuing as simscore: %d < defaultSimScore: %d", i, simscore, defaultSimScore)
 							continue IK
 						}
 					}
 
 					solutions[score] = step
 					if len(solutions) >= nSolutions {
 						// sufficient solutions found, stopping early
+						mp.logger.CDebugf(ctx, "getSolutions i: %d, sufficient solutions found, stopping early: len(solutions): %d", i, len(solutions))
 						break IK
 					}
 
-					if len(solutions) == 1 {
-						firstSolutionTime = time.Since(startTime)
-					} else {
-						elapsed := time.Since(startTime)
-						if elapsed > (time.Duration(mp.planOpts.TimeMultipleAfterFindingFirstSolution) * firstSolutionTime) {
-							mp.logger.Infof("ending early because of time elapsed: %v firstSolutionTime: %v", elapsed, firstSolutionTime)
-							break IK
-						}
-					}
+					// if len(solutions) == 1 {
+					// 	firstSolutionTime = time.Since(startTime)
+					// } else {
+					// 	elapsed := time.Since(startTime)
+					// 	if elapsed > (time.Duration(mp.planOpts.TimeMultipleAfterFindingFirstSolution) * firstSolutionTime) {
+					// 		mp.logger.Infof("ending early because of time elapsed: %v firstSolutionTime: %v", elapsed, firstSolutionTime)
+					// 		break IK
+					// 	}
+					// }
 				} else {
 					constraintFailCnt++
 					failures[err.Error()]++
@@ -648,9 +660,14 @@ IK:
 		}
 
 		select {
-		case <-ikErr:
+		case err := <-ikErr:
 			// If we have a return from the IK solver, there are no more solutions, so we finish processing above
 			// until we've drained the channel, handled by the `continue` above
+			if err != nil {
+				mp.logger.CDebugf(ctx, "getSolutions i: %d, ikErr: %s", i, err.Error())
+			} else {
+				mp.logger.CDebugf(ctx, "getSolutions i: %d, ikErr is nil", i)
+			}
 			break IK
 		default:
 		}

motionplan/armplanning/planManager.go

@@ -97,8 +97,10 @@ func (pm *planManager) planMultiWaypoint(ctx context.Context, seedPlan motionpla
 		waypoints = append(waypoints, goalWaypoints...)
 	}
 
+	pm.logger.Debug("planAtomicWaypoints start")
 	plan, err := pm.planAtomicWaypoints(ctx, waypoints, seedPlan)
 	pm.activeBackgroundWorkers.Wait()
+	pm.logger.Debug("planAtomicWaypoints stop")
 	if err != nil {
 		if len(waypoints) > 1 {
 			err = fmt.Errorf("failed to plan path for valid goal: %w", err)
@@ -445,6 +447,8 @@ func (pm *planManager) planParallelRRTMotion(
 
 // generateWaypoints will return the list of atomic waypoints that correspond to a specific goal in a plan request.
 func (pm *planManager) generateWaypoints(seedPlan motionplan.Plan, wpi int) ([]atomicWaypoint, error) {
+	pm.logger.Debug("generateWaypoints start")
+	defer pm.logger.Debug("generateWaypoints end")
 	wpGoals := pm.request.Goals[wpi]
 	startState := pm.request.StartState
 	if wpi > 0 {

motionplan/armplanning/plannerOptions.go

@@ -66,13 +66,12 @@ const (
 	// var defaultPlanner = newCBiRRTMotionPlanner.
 )
 
-var (
-	defaultNumThreads                            = utils.MinInt(runtime.NumCPU()/2, 10)
-	defaultTimeMultipleAfterFindingFirstSolution = 10
-)
+var defaultNumThreads = runtime.NumCPU() / 2
+
+// defaultTimeMultipleAfterFindingFirstSolution = 10
 
 func init() {
-	defaultTimeMultipleAfterFindingFirstSolution = utils.GetenvInt("MP_TIME_MULTIPLIER", defaultTimeMultipleAfterFindingFirstSolution)
+	// defaultTimeMultipleAfterFindingFirstSolution = utils.GetenvInt("MP_TIME_MULTIPLIER", defaultTimeMultipleAfterFindingFirstSolution)
 	defaultNumThreads = utils.GetenvInt("MP_NUM_THREADS", defaultNumThreads)
 }
 
@@ -115,7 +114,7 @@ func NewBasicPlannerOptions() *PlannerOptions {
 
 	opt.SmoothIter = defaultSmoothIter
 
-	opt.TimeMultipleAfterFindingFirstSolution = defaultTimeMultipleAfterFindingFirstSolution
+	// opt.TimeMultipleAfterFindingFirstSolution = defaultTimeMultipleAfterFindingFirstSolution
 	opt.NumThreads = defaultNumThreads
 
 	opt.LineTolerance = defaultLinearDeviation
@@ -237,7 +236,7 @@ type PlannerOptions struct {
 	// For inverse kinematics, the time within which each pending solution must finish its computation is
 	// a multiple of the time taken to compute the first solution. This parameter is a way to
 	// set that multiplicative factor.
-	TimeMultipleAfterFindingFirstSolution int `json:"time_multiple_after_finding_first_solution"`
+	// TimeMultipleAfterFindingFirstSolution int `json:"time_multiple_after_finding_first_solution"`
 }
 
 // NewPlannerOptionsFromExtra returns basic default settings updated by overridden parameters

motionplan/ik/combinedInverseKinematics_cgo.go

@@ -56,6 +56,8 @@ func (ik *combinedIK) Solve(ctx context.Context,
 	m func([]float64) float64,
 	rseed int,
 ) error {
+	ik.logger.CDebugf(ctx, "combinedIK Solve start")
+	defer ik.logger.CDebugf(ctx, "combinedIK Solve end")
 	var err error
 	ctxWithCancel, cancel := context.WithCancel(ctx)
 	defer cancel()

motionplan/ik/nloptInverseKinematics.go

@@ -89,6 +89,8 @@ func (ik *nloptIK) Solve(ctx context.Context,
 	minFunc func([]float64) float64,
 	rseed int,
 ) error {
+	ik.logger.CDebugf(ctx, "nloptIK Solve start")
+	defer ik.logger.CDebugf(ctx, "nloptIK Solve end")
 	if len(seed) != len(ik.limits) {
 		return fmt.Errorf("nlopt initialized with %d dof but seed was length %d", len(ik.limits), len(seed))
 	}

referenceframe/frame.go

@@ -9,6 +9,7 @@ import (
 	"fmt"
 	"math"
 	"math/rand"
+	"reflect"
 	"strings"
 
 	"github.com/golang/geo/r3"
@@ -29,6 +30,15 @@ type Limit struct {
 	Max float64
 }
 
+func limitAlmostEqual(limit1, limit2 Limit, epsilon float64) bool {
+	if math.Abs(limit1.Max-limit2.Max) > epsilon {
+		return false
+	} else if math.Abs(limit1.Min-limit2.Min) > epsilon {
+		return false
+	}
+	return true
+}
+
 // RestrictedRandomFrameInputs will produce a list of valid, in-bounds inputs for the frame.
 // The range of selection is restricted to `restrictionPercent` percent of the limits, and the
 // selection frame is centered at reference.
@@ -93,6 +103,18 @@ type Limited interface {
 	DoF() []Limit
 }
 
+func limitsAlmostEqual(limits1, limits2 []Limit, epsilon float64) bool {
+	if len(limits1) != len(limits2) {
+		return false
+	}
+	for i, limit := range limits1 {
+		if !limitAlmostEqual(limit, limits2[i], epsilon) {
+			return false
+		}
+	}
+	return true
+}
+
 // Frame represents a reference frame, e.g. an arm, a joint, a gripper, a board, etc.
 type Frame interface {
 	Limited
@@ -193,6 +215,17 @@ func (sf *tailGeometryStaticFrame) Geometries(input []Input) (*GeometriesInFrame
 	return NewGeometriesInFrame(sf.name, []spatial.Geometry{newGeom}), nil
 }
 
+func (sf *tailGeometryStaticFrame) UnmarshalJSON(data []byte) error {
+	var inner staticFrame
+
+	err := json.Unmarshal(data, &inner)
+	if err != nil {
+		return err
+	}
+	sf.staticFrame = &inner
+	return nil
+}
+
 // namedFrame is used to change the name of a frame.
 type namedFrame struct {
 	Frame
@@ -501,7 +534,9 @@ func (rf *rotationalFrame) UnmarshalJSON(data []byte) error {
 		return err
 	}
 
-	rf.baseFrame = &baseFrame{name: rf.Name(), limits: []Limit{{Min: cfg.Min, Max: cfg.Max}}}
+	rf.baseFrame = &baseFrame{name: cfg.ID, limits: []Limit{
+		{Min: utils.DegToRad(cfg.Min), Max: utils.DegToRad(cfg.Max)},
+	}}
 	rotAxis := cfg.Axis.ParseConfig()
 	rf.rotAxis = r3.Vector{X: rotAxis.RX, Y: rotAxis.RY, Z: rotAxis.RZ}
 	return nil
@@ -630,3 +665,85 @@ func PoseToInputs(p spatial.Pose) []Input {
 		p.Orientation().OrientationVectorRadians().Theta,
 	})
 }
+
+// Determine whether two frames are (nearly) identical. For now, we only support implementers of
+// the frame interface that are registered (see register.go). However, this is not automatic, so
+// if a new implementer is registered manually in register.go, its case should be added here.
+//
+// NOTE: for ease, this function only takes one epsilon parameter because we have yet
+// to see a case of quantity where we want accept different levels of floating point error.
+// If the time comes where we want a different allowance for limits, vectors, and geometries,
+// this function should be changed accordingly.
+func framesAlmostEqual(frame1, frame2 Frame, epsilon float64) (bool, error) {
+	switch {
+	case reflect.TypeOf(frame1) != reflect.TypeOf(frame2):
+		return false, nil
+	case frame1.Name() != frame2.Name():
+		return false, nil
+	case !limitsAlmostEqual(frame1.DoF(), frame2.DoF(), epsilon):
+		return false, nil
+	default:
+	}
+
+	if frame1 == nil {
+		return frame2 == nil, nil
+	} else if frame2 == nil {
+		return false, nil
+	}
+
+	switch f1 := frame1.(type) {
+	case *staticFrame:
+		f2 := frame2.(*staticFrame)
+		switch {
+		case !spatial.PoseAlmostEqual(f1.transform, f2.transform):
+			return false, nil
+		case !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry):
+			return false, nil
+		default:
+		}
+	case *rotationalFrame:
+		f2 := frame2.(*rotationalFrame)
+		if !spatial.R3VectorAlmostEqual(f1.rotAxis, f2.rotAxis, epsilon) {
+			return false, nil
+		}
+	case *translationalFrame:
+		f2 := frame2.(*translationalFrame)
+		switch {
+		case !spatial.R3VectorAlmostEqual(f1.transAxis, f2.transAxis, epsilon):
+			return false, nil
+		case !spatial.GeometriesAlmostEqual(f1.geometry, f2.geometry):
+			return false, nil
+		default:
+		}
+	case *tailGeometryStaticFrame:
+		f2 := frame2.(*tailGeometryStaticFrame)
+		switch {
+		case f1.staticFrame == nil:
+			return f2.staticFrame == nil, nil
+		case f2.staticFrame == nil:
+			return f1.staticFrame == nil, nil
+		default:
+			return framesAlmostEqual(f1.staticFrame, f2.staticFrame, epsilon)
+		}
+	case *SimpleModel:
+		f2 := frame2.(*SimpleModel)
+		ordTransforms1 := f1.OrdTransforms
+		ordTransforms2 := f2.OrdTransforms
+		if len(ordTransforms1) != len(ordTransforms2) {
+			return false, nil
+		} else {
+			for i, f := range ordTransforms1 {
+				frameEquality, err := framesAlmostEqual(f, ordTransforms2[i], epsilon)
+				if err != nil {
+					return false, err
+				}
+				if !frameEquality {
+					return false, nil
+				}
+			}
+		}
+	default:
+		return false, fmt.Errorf("equality conditions not defined for %t", frame1)
+	}
+	return true, nil
+}

referenceframe/frame_json.go

@@ -199,16 +199,23 @@ func jsonToFrame(data json.RawMessage) (Frame, error) {
 	if err := json.Unmarshal(sF["frame_type"], &frameType); err != nil {
 		return nil, err
 	}
+	if _, ok := sF["frame"]; !ok {
+		return nil, fmt.Errorf("no frame data found for frame, type was %s", frameType)
+	}
 
 	implementer, ok := registeredFrameImplementers[frameType]
 	if !ok {
 		return nil, fmt.Errorf("%s is not a registered Frame implementation", frameType)
 	}
 	frameZeroStruct := reflect.New(implementer).Elem()
-	if err := json.Unmarshal(sF["frame"], frameZeroStruct.Addr().Interface()); err != nil {
+	frame := frameZeroStruct.Addr().Interface()
+	frameI, err := utils.AssertType[Frame](frame)
+	if err != nil {
+		return nil, err
+	}
+	if err := json.Unmarshal(sF["frame"], frame); err != nil {
 		return nil, err
 	}
-	frame := frameZeroStruct.Addr().Interface()
 
-	return utils.AssertType[Frame](frame)
+	return frameI, nil
 }