From 26343aec03b99913744ce612527d4322fb921583 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 15 Oct 2025 09:57:16 -0400 Subject: [PATCH 01/20] wip --- motionplan/armplanning/cBiRRT.go | 12 +- .../armplanning/linearized_frame_system.go | 1 + motionplan/armplanning/node.go | 12 +- motionplan/armplanning/real_test.go | 5 +- motionplan/armplanning/smart_seed.go | 167 ++++++++++++++++++ motionplan/armplanning/smart_seed_test.go | 70 ++++++++ motionplan/metrics.go | 12 +- referenceframe/frame.go | 15 ++ 8 files changed, 277 insertions(+), 17 deletions(-) create mode 100644 motionplan/armplanning/smart_seed.go create mode 100644 motionplan/armplanning/smart_seed_test.go diff --git a/motionplan/armplanning/cBiRRT.go b/motionplan/armplanning/cBiRRT.go index 0d93243ef3f..524868a0d76 100644 --- a/motionplan/armplanning/cBiRRT.go +++ b/motionplan/armplanning/cBiRRT.go @@ -351,17 +351,7 @@ func (mp *cBiRRTMotionPlanner) getFrameSteps(percentTotalMovement float64, itera pos := make([]float64, len(dof)) for i, lim := range dof { - l, u := lim.Min, lim.Max - - // Default to [-999,999] as range if limits are infinite - if l == math.Inf(-1) { - l = -999 - } - if u == math.Inf(1) { - u = 999 - } - - jRange := math.Abs(u - l) + _, _, jRange := lim.GoodLimits() pos[i] = jRange * percentTotalMovement if isMoving { diff --git a/motionplan/armplanning/linearized_frame_system.go b/motionplan/armplanning/linearized_frame_system.go index 201a6642179..43f6a6ee204 100644 --- a/motionplan/armplanning/linearized_frame_system.go +++ b/motionplan/armplanning/linearized_frame_system.go @@ -172,3 +172,4 @@ func (lfs *linearizedFrameSystem) jog(idx int, val, percentJog float64) float64 return val } + diff --git a/motionplan/armplanning/node.go b/motionplan/armplanning/node.go index 71d7ff46325..96d5ffbe109 100644 --- a/motionplan/armplanning/node.go +++ b/motionplan/armplanning/node.go @@ -142,7 +142,17 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er sss.maxSolutions = defaultSolutionsToSeed } - sss.linearSeed, err = psc.pc.lfs.mapToSlice(psc.start) + ssc, err := smartSeed(psc.pc.fs) + if err != nil { + return nil, err + } + + seed, err := ssc.findSeed(psc.goal, psc.start) + if err != nil { + return nil, err + } + + sss.linearSeed, err = psc.pc.lfs.mapToSlice(seed) if err != nil { return nil, err } diff --git a/motionplan/armplanning/real_test.go b/motionplan/armplanning/real_test.go index 457d23c8f27..39971ab3d1a 100644 --- a/motionplan/armplanning/real_test.go +++ b/motionplan/armplanning/real_test.go @@ -147,7 +147,7 @@ func TestSandingLargeMove1(t *testing.T) { } func TestPirouette(t *testing.T) { - t.Skip() + // t.Skip() // get arm kinematics for forward kinematics armName := "ur5e" @@ -187,7 +187,7 @@ func TestPirouette(t *testing.T) { fs := referenceframe.NewEmptyFrameSystem("pirouette") err = fs.AddFrame(armKinematics, fs.World()) test.That(t, err, test.ShouldBeNil) - + for iter := 0; iter < 10; iter++ { // keep track of previous index of idealJointValues, used for calculating expected joint 0 change prevIndex := 0 @@ -201,6 +201,7 @@ func TestPirouette(t *testing.T) { logger := logging.NewTestLogger(t) // construct req and get the plan goalState := NewPlanState(map[string]*referenceframe.PoseInFrame{armName: p}, nil) + req := &PlanRequest{ FrameSystem: fs, Goals: []*PlanState{goalState}, diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go new file mode 100644 index 00000000000..5f15a96bb1c --- /dev/null +++ b/motionplan/armplanning/smart_seed.go @@ -0,0 +1,167 @@ +package armplanning + +import ( + "fmt" + "sort" + + "go.viam.com/rdk/motionplan" + "go.viam.com/rdk/referenceframe" +) + +type smartSeedCacheEntry struct { + inputs referenceframe.FrameSystemInputs + poses referenceframe.FrameSystemPoses +} + +type smartSeedCache struct { + fs *referenceframe.FrameSystem + lfs *linearizedFrameSystem + + cache []smartSeedCacheEntry +} + +func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start referenceframe.FrameSystemInputs) (referenceframe.FrameSystemInputs, error) { + for _, p := range goal { + if p.Parent() != "world" { + return nil, fmt.Errorf("goal has to be in world, not %s", p.Parent()) + } + } + + type entry struct { + idx int + distance float64 + cost float64 + } + + best := []entry{} + + + bestDistance := 10000000000.0 + + for idx, c := range ssc.cache { + distance := 0.0 + for k, p := range goal { + distance += motionplan.WeightedSquaredNormDistance(p.Pose(), c.poses[k].Pose()) + if distance > (bestDistance * 2) { + break + } + + } + + if distance < bestDistance { + bestDistance = distance + } + + if distance > (bestDistance * 2) { + continue + } + + cost := 0.0 + for k, j := range start { + cost += referenceframe.InputsL2Distance(j, c.inputs[k]) + } + + best = append(best, entry{idx, distance, cost}) + } + + sort.Slice(best, func(i,j int) bool { + return best[i].distance < best[j].distance + }) + + cutIdx := 0 + for cutIdx < len(best) { + if best[cutIdx].distance > (2 * best[0].distance) { + break + } + cutIdx++ + } + + best = best[0:cutIdx] + + sort.Slice(best, func(i,j int) bool { + return best[i].cost < best[j].cost + }) + /* + for i := 0; i < len(best) && i < 100; i++ { + e := best[i] + fmt.Printf("%v dist: %02.f cost: %0.2f\n", ssc.cache[e.idx].inputs, e.distance, e.cost) + } + */ + return ssc.cache[best[0].idx].inputs, nil +} + + +func (ssc *smartSeedCache) addToCache(values []float64) error { + inputs, err := ssc.lfs.sliceToMap(values) + if err != nil { + return err + } + poses, err := inputs.ComputePoses(ssc.fs) + if err != nil { + return err + } + + for _, p := range poses { + if p.Parent() != "world" { + return fmt.Errorf("why not in world, but %s", p.Parent()) + } + } + + ssc.cache = append(ssc.cache, smartSeedCacheEntry{inputs, poses}) + return nil +} + +func (ssc *smartSeedCache) build(values []float64, joint int) error { + if joint > len(ssc.lfs.dof) { + panic(fmt.Errorf("joint: %d > len(ssc.lfs.dof): %d", joint, len(ssc.lfs.dof))) + } + + if joint == len(ssc.lfs.dof) { + return ssc.addToCache(values) + } + + min, max, r := ssc.lfs.dof[joint].GoodLimits() + values[joint] = min + + jog := r / 10 + for values[joint] <= max { + err := ssc.build(values, joint + 1) + if err != nil { + return err + } + values[joint] += jog + } + return nil +} + +var foooooo map[*referenceframe.FrameSystem]*smartSeedCache + +func smartSeed(fs *referenceframe.FrameSystem) (*smartSeedCache, error) { + if foooooo == nil { + foooooo = map[*referenceframe.FrameSystem]*smartSeedCache{} + } + c, ok := foooooo[fs] + fmt.Printf("ok: %v\n", ok) + if ok { + return c, nil + } + + lfs, err := newLinearizedFrameSystem(fs) + if err != nil { + return nil, err + } + + c = &smartSeedCache{ + fs: fs, + lfs: lfs, + } + + values := make([]float64, len(lfs.dof)) + err = c.build(values, 0) + if err != nil { + return nil, err + } + + foooooo[fs] = c + return c, nil +} diff --git a/motionplan/armplanning/smart_seed_test.go b/motionplan/armplanning/smart_seed_test.go new file mode 100644 index 00000000000..eeada01f05a --- /dev/null +++ b/motionplan/armplanning/smart_seed_test.go @@ -0,0 +1,70 @@ +package armplanning + +import ( + "testing" + + "github.com/golang/geo/r3" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" + "go.viam.com/test" +) + +func TestSmartSeedCache1(t *testing.T) { + armName := "ur5e" + armKinematics, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/kinematics/ur5e.json"), armName) + test.That(t, err, test.ShouldBeNil) + + fs := referenceframe.NewEmptyFrameSystem("pirouette") + err = fs.AddFrame(armKinematics, fs.World()) + test.That(t, err, test.ShouldBeNil) + + c, err := smartSeed(fs) + test.That(t, err, test.ShouldBeNil) + + start := referenceframe.FrameSystemInputs{"ur5e" : {{1.0471667423817}, {0.011108350341463286}, {-1.0899013011625651}, {-3.0938870331059594}, {-1.767558957758243e-05}, {-3.681258507284093}}} + + seed, err := c.findSeed( + referenceframe.FrameSystemPoses{"ur5e": referenceframe.NewPoseInFrame("world", + spatialmath.NewPose( + r3.Vector{X:-337.976430, Y:-464.051182, Z:554.695381}, + &spatialmath.OrientationVectorDegrees{OX:0.499987, OY:-0.866033, OZ:-0.000000, Theta:0.000000}, + ))}, + start) + test.That(t, err, test.ShouldBeNil) + + cost := referenceframe.InputsL2Distance(start["ur5e"], seed["ur5e"]) + test.That(t, cost, test.ShouldBeLessThan, 1.25) + +} + +func BenchmarkSmartSeedCacheSearch(t *testing.B) { + + armName := "ur5e" + armKinematics, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/kinematics/ur5e.json"), armName) + test.That(t, err, test.ShouldBeNil) + + fs := referenceframe.NewEmptyFrameSystem("pirouette") + err = fs.AddFrame(armKinematics, fs.World()) + test.That(t, err, test.ShouldBeNil) + + c, err := smartSeed(fs) + test.That(t, err, test.ShouldBeNil) + + start := referenceframe.FrameSystemInputs{"ur5e" : {{1.0471667423817}, {0.011108350341463286}, {-1.0899013011625651}, {-3.0938870331059594}, {-1.767558957758243e-05}, {-3.681258507284093}}} + + t.ResetTimer() + + for range t.N { + _, err = c.findSeed( + referenceframe.FrameSystemPoses{"ur5e": referenceframe.NewPoseInFrame("world", + spatialmath.NewPose( + r3.Vector{X:-337.976430, Y:-464.051182, Z:554.695381}, + &spatialmath.OrientationVectorDegrees{OX:0.499987, OY:-0.866033, OZ:-0.000000, Theta:0.000000}, + ))}, + start) + test.That(t, err, test.ShouldBeNil) + } + +} diff --git a/motionplan/metrics.go b/motionplan/metrics.go index 4ec86482470..e9a8ad32d3e 100644 --- a/motionplan/metrics.go +++ b/motionplan/metrics.go @@ -297,16 +297,22 @@ func SquaredNormNoOrientSegmentMetric(segment *Segment) float64 { // This changes the magnitude of the position delta used to be smaller and avoid numeric instability issues that happens with large floats. // It also scales the orientation distance to give more weight to it. func WeightedSquaredNormSegmentMetric(segment *Segment) float64 { + return WeightedSquaredNormDistance(segment.StartPosition, segment.EndPosition) +} + +// WeightedSquaredNormDistance is a distance function between two poses to be used for gradient descent. +func WeightedSquaredNormDistance(start, end spatial.Pose) float64 { // Increase weight for orientation since it's a small number orientDelta := spatial.QuatToR3AA(spatial.OrientationBetween( - segment.EndPosition.Orientation(), - segment.StartPosition.Orientation(), + start.Orientation(), + end.Orientation(), ).Quaternion()).Mul(orientationDistanceScaling).Norm2() // Also, we multiply delta.Point() by 0.1, effectively measuring in cm rather than mm. - ptDelta := segment.EndPosition.Point().Mul(0.1).Sub(segment.StartPosition.Point().Mul(0.1)).Norm2() + ptDelta := end.Point().Mul(0.1).Sub(start.Point().Mul(0.1)).Norm2() return ptDelta + orientDelta } + // TODO(RSDK-2557): Writing a PenetrationDepthMetric will allow cbirrt to path along the sides of obstacles rather than terminating // the RRT tree when an obstacle is hit diff --git a/referenceframe/frame.go b/referenceframe/frame.go index e3a46069caa..87a2f6b3cda 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -35,6 +35,21 @@ func (l *Limit) Range() float64 { return l.Max - l.Min } +const rangeLimit = 999 + +// GoodLimits gives min, max, range, but capped to -999,999 +func (l *Limit) GoodLimits() (float64, float64, float64) { + a := l.Min + b := l.Max + if a < -1 * rangeLimit { + a = -1 * rangeLimit + } + if b > rangeLimit { + b = rangeLimit + } + return a, b, b-a +} + func limitsAlmostEqual(limits1, limits2 []Limit, epsilon float64) bool { if len(limits1) != len(limits2) { return false From 8ade1173194bf61b0d49dca98d3d35aaa414a020 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 15 Oct 2025 10:51:28 -0400 Subject: [PATCH 02/20] wip2 --- motionplan/armplanning/node.go | 2 +- motionplan/armplanning/smart_seed.go | 124 +++++++++++++++++----- motionplan/armplanning/smart_seed_test.go | 10 +- 3 files changed, 105 insertions(+), 31 deletions(-) diff --git a/motionplan/armplanning/node.go b/motionplan/armplanning/node.go index 96d5ffbe109..612407c5a97 100644 --- a/motionplan/armplanning/node.go +++ b/motionplan/armplanning/node.go @@ -147,7 +147,7 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er return nil, err } - seed, err := ssc.findSeed(psc.goal, psc.start) + seed, err := ssc.findSeed(psc.goal, psc.start, psc.pc.logger) if err != nil { return nil, err } diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index 5f15a96bb1c..533c64dafc2 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -3,7 +3,10 @@ package armplanning import ( "fmt" "sort" + + "github.com/golang/geo/r3" + "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" "go.viam.com/rdk/referenceframe" ) @@ -17,10 +20,16 @@ type smartSeedCache struct { fs *referenceframe.FrameSystem lfs *linearizedFrameSystem - cache []smartSeedCacheEntry + rawCache []smartSeedCacheEntry + + //newCache map[string]map[string][]smartSeedCacheEntry } -func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start referenceframe.FrameSystemInputs) (referenceframe.FrameSystemInputs, error) { +func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start referenceframe.FrameSystemInputs, logger logging.Logger) (referenceframe.FrameSystemInputs, error) { + if len(goal) > 1 { + return nil, fmt.Errorf("smartSeedCache findSeed only works with 1 goal for now") + } + for _, p := range goal { if p.Parent() != "world" { return nil, fmt.Errorf("goal has to be in world, not %s", p.Parent()) @@ -28,7 +37,7 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start } type entry struct { - idx int + e *smartSeedCacheEntry distance float64 cost float64 } @@ -36,34 +45,39 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start best := []entry{} - bestDistance := 10000000000.0 - - for idx, c := range ssc.cache { - distance := 0.0 - for k, p := range goal { - distance += motionplan.WeightedSquaredNormDistance(p.Pose(), c.poses[k].Pose()) - if distance > (bestDistance * 2) { - break - } + startPoses, err := start.ComputePoses(ssc.fs) + if err != nil { + return nil, err + } + startDistance := ssc.distance(startPoses, goal) + logger.Debugf("startDistance: %v", startDistance) + + bestDistance := startDistance * 2 + + for _, c := range ssc.rawCache { + distance := ssc.distance(goal, c.poses) + if distance > (bestDistance * 2) { + continue } if distance < bestDistance { bestDistance = distance } - - if distance > (bestDistance * 2) { - continue - } cost := 0.0 for k, j := range start { cost += referenceframe.InputsL2Distance(j, c.inputs[k]) } - best = append(best, entry{idx, distance, cost}) + best = append(best, entry{&c, distance, cost}) } + if len(best) == 0 { + logger.Debugf("no best, returning start") + return start, nil + } + sort.Slice(best, func(i,j int) bool { return best[i].distance < best[j].distance }) @@ -81,15 +95,35 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start sort.Slice(best, func(i,j int) bool { return best[i].cost < best[j].cost }) - /* - for i := 0; i < len(best) && i < 100; i++ { + + + for i := 0; i < len(best) && i < 5; i++ { e := best[i] - fmt.Printf("%v dist: %02.f cost: %0.2f\n", ssc.cache[e.idx].inputs, e.distance, e.cost) + logger.Debugf("%v dist: %02.f cost: %0.2f", e.e.inputs, e.distance, e.cost) } - */ - return ssc.cache[best[0].idx].inputs, nil + + return best[0].e.inputs, nil } +func (ssc *smartSeedCache) distance(a, b referenceframe.FrameSystemPoses) float64 { + dist := 0.0 + + for k, p := range a { + if p.Parent() != "world" { + panic(fmt.Errorf("eliot fucked up %s", p.Parent())) + } + + pp := b[k] + + if pp.Parent() != "world" { + panic(fmt.Errorf("eliot fucked up %s", pp.Parent())) + } + + dist += motionplan.WeightedSquaredNormDistance(p.Pose(), pp.Pose()) + } + + return dist +} func (ssc *smartSeedCache) addToCache(values []float64) error { inputs, err := ssc.lfs.sliceToMap(values) @@ -107,11 +141,11 @@ func (ssc *smartSeedCache) addToCache(values []float64) error { } } - ssc.cache = append(ssc.cache, smartSeedCacheEntry{inputs, poses}) + ssc.rawCache = append(ssc.rawCache, smartSeedCacheEntry{inputs, poses}) return nil } -func (ssc *smartSeedCache) build(values []float64, joint int) error { +func (ssc *smartSeedCache) buildRawCache(values []float64, joint int) error { if joint > len(ssc.lfs.dof) { panic(fmt.Errorf("joint: %d > len(ssc.lfs.dof): %d", joint, len(ssc.lfs.dof))) } @@ -125,7 +159,7 @@ func (ssc *smartSeedCache) build(values []float64, joint int) error { jog := r / 10 for values[joint] <= max { - err := ssc.build(values, joint + 1) + err := ssc.buildRawCache(values, joint + 1) if err != nil { return err } @@ -134,6 +168,42 @@ func (ssc *smartSeedCache) build(values []float64, joint int) error { return nil } +func (ssc *smartSeedCache) buildCache() error { + values := make([]float64, len(ssc.lfs.dof)) + err := ssc.buildRawCache(values, 0) + if err != nil { + return fmt.Errorf("cannot buildCache: %w", err) + } + + for frame, _ := range ssc.rawCache[0].poses { + err = ssc.buildInverseCache(frame) + if err != nil { + return fmt.Errorf("cannot buildInverseCache for %s: %w", frame, err) + } + } + + return nil +} + +func (ssc *smartSeedCache) buildInverseCache(frame string) error { + var minCartesian, maxCartesian r3.Vector + + for _, e := range ssc.rawCache { + minCartesian.X = min(minCartesian.X, e.poses[frame].Pose().Point().X) + minCartesian.Y = min(minCartesian.X, e.poses[frame].Pose().Point().Y) + minCartesian.Z = min(minCartesian.X, e.poses[frame].Pose().Point().X) + + maxCartesian.X = max(maxCartesian.X, e.poses[frame].Pose().Point().X) + maxCartesian.Y = max(maxCartesian.X, e.poses[frame].Pose().Point().Y) + maxCartesian.Z = max(maxCartesian.X, e.poses[frame].Pose().Point().X) + } + + fmt.Printf("%v -> %v %v\n", frame, minCartesian, maxCartesian) + + return nil + +} + var foooooo map[*referenceframe.FrameSystem]*smartSeedCache func smartSeed(fs *referenceframe.FrameSystem) (*smartSeedCache, error) { @@ -156,8 +226,8 @@ func smartSeed(fs *referenceframe.FrameSystem) (*smartSeedCache, error) { lfs: lfs, } - values := make([]float64, len(lfs.dof)) - err = c.build(values, 0) + + err = c.buildCache() if err != nil { return nil, err } diff --git a/motionplan/armplanning/smart_seed_test.go b/motionplan/armplanning/smart_seed_test.go index eeada01f05a..1d2afd04d68 100644 --- a/motionplan/armplanning/smart_seed_test.go +++ b/motionplan/armplanning/smart_seed_test.go @@ -5,6 +5,7 @@ import ( "github.com/golang/geo/r3" + "go.viam.com/rdk/logging" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" @@ -12,6 +13,8 @@ import ( ) func TestSmartSeedCache1(t *testing.T) { + logger := logging.NewTestLogger(t) + armName := "ur5e" armKinematics, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/kinematics/ur5e.json"), armName) test.That(t, err, test.ShouldBeNil) @@ -31,7 +34,7 @@ func TestSmartSeedCache1(t *testing.T) { r3.Vector{X:-337.976430, Y:-464.051182, Z:554.695381}, &spatialmath.OrientationVectorDegrees{OX:0.499987, OY:-0.866033, OZ:-0.000000, Theta:0.000000}, ))}, - start) + start, logger) test.That(t, err, test.ShouldBeNil) cost := referenceframe.InputsL2Distance(start["ur5e"], seed["ur5e"]) @@ -40,7 +43,8 @@ func TestSmartSeedCache1(t *testing.T) { } func BenchmarkSmartSeedCacheSearch(t *testing.B) { - + logger := logging.NewTestLogger(t) + armName := "ur5e" armKinematics, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/kinematics/ur5e.json"), armName) test.That(t, err, test.ShouldBeNil) @@ -63,7 +67,7 @@ func BenchmarkSmartSeedCacheSearch(t *testing.B) { r3.Vector{X:-337.976430, Y:-464.051182, Z:554.695381}, &spatialmath.OrientationVectorDegrees{OX:0.499987, OY:-0.866033, OZ:-0.000000, Theta:0.000000}, ))}, - start) + start, logger) test.That(t, err, test.ShouldBeNil) } From dcdd4cfbb7ddd53568a55868671a5f4969002adc Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 15 Oct 2025 17:01:09 -0400 Subject: [PATCH 03/20] wip --- motionplan/armplanning/node.go | 103 +++++++------- motionplan/armplanning/smart_seed.go | 164 +++++++++++++++++----- motionplan/armplanning/smart_seed_test.go | 9 +- motionplan/ik/combined.go | 13 +- 4 files changed, 197 insertions(+), 92 deletions(-) diff --git a/motionplan/armplanning/node.go b/motionplan/armplanning/node.go index 612407c5a97..a74863ddd41 100644 --- a/motionplan/armplanning/node.go +++ b/motionplan/armplanning/node.go @@ -15,8 +15,6 @@ import ( "go.viam.com/rdk/referenceframe" ) -const ikTimeMultipleStart = 70 - // fixedStepInterpolation returns inputs at qstep distance along the path from start to target. func fixedStepInterpolation(start, target *node, qstep map[string][]float64) referenceframe.FrameSystemInputs { newNear := make(referenceframe.FrameSystemInputs) @@ -108,6 +106,7 @@ type solutionSolvingState struct { psc *planSegmentContext maxSolutions int + seed referenceframe.FrameSystemInputs linearSeed []float64 moving, nonmoving []string @@ -120,7 +119,6 @@ type solutionSolvingState struct { solutions []*node startTime time.Time bestScore float64 - ikTimeMultiple int firstSolutionTime time.Duration } @@ -129,30 +127,34 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er sss := &solutionSolvingState{ psc: psc, + seed: psc.start, solutions: []*node{}, failures: newIkConstraintError(psc.pc.fs, psc.checker), startTime: time.Now(), firstSolutionTime: time.Hour, bestScore: 10000000, maxSolutions: psc.pc.planOpts.MaxSolutions, - ikTimeMultiple: ikTimeMultipleStart, // look for a while, unless we find good things } if sss.maxSolutions <= 0 { sss.maxSolutions = defaultSolutionsToSeed } - ssc, err := smartSeed(psc.pc.fs) - if err != nil { - return nil, err - } - - seed, err := ssc.findSeed(psc.goal, psc.start, psc.pc.logger) - if err != nil { - return nil, err + if len(psc.pc.lfs.dof) <= 6 { + ssc, err := smartSeed(psc.pc.fs) + if err != nil { + return nil, err + } + + sss.seed, err = ssc.findSeed(psc.goal, psc.start, psc.pc.logger) + if err != nil { + return nil, err + } } + psc.pc.logger.Debugf("psc.start -> seed: \n%v\n%v", psc.start, sss.seed) - sss.linearSeed, err = psc.pc.lfs.mapToSlice(seed) + + sss.linearSeed, err = psc.pc.lfs.mapToSlice(sss.seed) if err != nil { return nil, err } @@ -168,7 +170,7 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er } func (sss *solutionSolvingState) computeGoodCost(goal referenceframe.FrameSystemPoses) error { - sss.ratios = sss.psc.pc.lfs.inputChangeRatio(sss.psc.motionChains, sss.psc.start, + sss.ratios = sss.psc.pc.lfs.inputChangeRatio(sss.psc.motionChains, sss.seed, sss.psc.pc.planOpts.getGoalMetric(goal), sss.psc.pc.logger) adjusted := []float64{} @@ -274,57 +276,62 @@ func (sss *solutionSolvingState) process(stepSolution *ik.Solution, } } + if len(sss.solutions) == 0 { + sss.firstSolutionTime = time.Since(sss.startTime) + } + + + myNode := &node{inputs: step, cost: sss.psc.pc.configurationDistanceFunc(stepArc)} sss.solutions = append(sss.solutions, myNode) - // TODO: Reevaluate this constant when better quality IK solutions are being generated. - const goodCostStopDivider = 4.0 + if myNode.cost < sss.bestScore { + sss.bestScore = myNode.cost + } - if myNode.cost < sss.goodCost || // this checks the absolute score of the plan - // if we've got something sane, and it's really good, let's check - myNode.cost < (sss.bestScore*defaultOptimalityMultiple) { + if myNode.cost < min(sss.goodCost, sss.bestScore*defaultOptimalityMultiple) { whyNot := sss.psc.checkPath(sss.psc.start, step) sss.psc.pc.logger.Debugf("got score %0.4f and goodCost: %0.2f - result: %v", myNode.cost, sss.goodCost, whyNot) - if whyNot == nil { - myNode.checkPath = true - if (myNode.cost < (sss.goodCost / goodCostStopDivider)) || - (myNode.cost < sss.psc.pc.planOpts.MinScore && sss.psc.pc.planOpts.MinScore > 0) { - sss.psc.pc.logger.Debugf("\tscore %0.4f stopping early (%0.2f) processCalls: %d after %v", - myNode.cost, sss.goodCost/goodCostStopDivider, sss.processCalls, time.Since(sss.startTime)) - return true // good solution, stopping early - } - - if myNode.cost < (sss.goodCost / (.5 * goodCostStopDivider)) { - // we find something very good, but not great - // so we look at lot - sss.ikTimeMultiple = min(sss.ikTimeMultiple, ikTimeMultipleStart/12) - } else if myNode.cost < sss.goodCost { - sss.ikTimeMultiple = min(sss.ikTimeMultiple, ikTimeMultipleStart/5) - } - } + myNode.checkPath = whyNot == nil } + return sss.shouldStopEarly() +} + +// return bool is if we should stop because we're done. +func (sss *solutionSolvingState) shouldStopEarly() bool { + elapsed := time.Since(sss.startTime) + if len(sss.solutions) >= sss.maxSolutions { + sss.psc.pc.logger.Debugf("stopping with %d solutions after: %v", len(sss.solutions), elapsed) return true } - if myNode.cost < sss.bestScore { - sss.bestScore = myNode.cost + if sss.bestScore < .2 { + sss.psc.pc.logger.Debugf("stopping early with amazing %0.2f after: %v", sss.bestScore, elapsed) + return true } - if len(sss.solutions) == 1 { - sss.firstSolutionTime = time.Since(sss.startTime) - } else { - elapsed := time.Since(sss.startTime) - if elapsed > (time.Duration(sss.ikTimeMultiple) * sss.firstSolutionTime) { - sss.psc.pc.logger.Infof("ending early because of time elapsed: %v firstSolutionTime: %v processCalls: %d", - elapsed, sss.firstSolutionTime, sss.processCalls) - return true - } + + if sss.bestScore < (sss.goodCost / 10) && elapsed > 100 * time.Millisecond { + sss.psc.pc.logger.Debugf("stopping early with bestScore %0.2f (%0.2f) after: %v", sss.bestScore, sss.goodCost, elapsed) + return true + } + + multiple := 50.0 + if sss.bestScore < sss.goodCost { + multiple *= (sss.bestScore / sss.goodCost) + } + + if elapsed > max(sss.firstSolutionTime * time.Duration(multiple), 100*time.Millisecond) { + sss.psc.pc.logger.Debugf("stopping early with bestScore %0.2f after: %v", sss.bestScore, elapsed) + return true } + return false } + // getSolutions will initiate an IK solver for the given position and seed, collect solutions, and // score them by constraints. // @@ -346,8 +353,6 @@ func getSolutions(ctx context.Context, psc *planSegmentContext) ([]*node, error) // Spawn the IK solver to generate solutions until done minFunc := psc.pc.linearizeFSmetric(psc.pc.planOpts.getGoalMetric(psc.goal)) - psc.pc.logger.Debugf("seed: %v", psc.start) - ctxWithCancel, cancel := context.WithCancel(ctx) defer cancel() diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index 533c64dafc2..9f74ef58b4d 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -3,12 +3,14 @@ package armplanning import ( "fmt" "sort" + "time" "github.com/golang/geo/r3" "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" ) type smartSeedCacheEntry struct { @@ -16,13 +18,67 @@ type smartSeedCacheEntry struct { poses referenceframe.FrameSystemPoses } +type goalCacheBox struct { + hash string + center r3.Vector + entries []smartSeedCacheEntry +} + +type goalCache struct { + minCartesian, maxCartesian r3.Vector + boxes map[string]*goalCacheBox // hash to list +} + +func (gc *goalCache) hashKey(value, min, max float64) int { + x := (value - min) / (max - min) + + d := int(x*10) + if d >= 10 { + d = 9 + } + return d +} + +func (gc *goalCache) hash(p r3.Vector) string { + x := gc.hashKey(p.X, gc.minCartesian.X, gc.maxCartesian.X) + y := gc.hashKey(p.Y, gc.minCartesian.Y, gc.maxCartesian.Y) + z := gc.hashKey(p.Z, gc.minCartesian.Z, gc.maxCartesian.Z) + return fmt.Sprintf("%d%d%d", x, y, z) +} + type smartSeedCache struct { fs *referenceframe.FrameSystem lfs *linearizedFrameSystem rawCache []smartSeedCacheEntry - //newCache map[string]map[string][]smartSeedCacheEntry + geoCache map[string]*goalCache +} + +func (scs *smartSeedCache) findBoxes(goalFrame string, goalPose spatialmath.Pose) []*goalCacheBox { + type e struct { + b *goalCacheBox + d float64 + } + + best := []e{} + + for _, b := range scs.geoCache[goalFrame].boxes { + d := goalPose.Point().Distance(b.center) + best = append(best, e{b,d}) + } + + sort.Slice(best, func(a,b int) bool { + return best[a].d < best[b].d + }) + + boxes := []*goalCacheBox{} + + for i := 0; i < 10 && i < len(best); i++ { + boxes = append(boxes, best[i].b) + } + + return boxes } func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start referenceframe.FrameSystemInputs, logger logging.Logger) (referenceframe.FrameSystemInputs, error) { @@ -36,6 +92,14 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start } } + goalFrame := "" + var goalPose spatialmath.Pose + + for k, v := range goal { + goalFrame = k + goalPose = v.Pose() + } + type entry struct { e *smartSeedCacheEntry distance float64 @@ -51,26 +115,29 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start } startDistance := ssc.distance(startPoses, goal) - logger.Debugf("startDistance: %v", startDistance) bestDistance := startDistance * 2 - - for _, c := range ssc.rawCache { - distance := ssc.distance(goal, c.poses) - if distance > (bestDistance * 2) { - continue - } - if distance < bestDistance { - bestDistance = distance + boxes := ssc.findBoxes(goalFrame, goalPose) + + for _, b := range boxes { + for _, c := range b.entries { + distance := ssc.distance(goal, c.poses) + if distance > (bestDistance * 2) { + continue + } + + if distance < bestDistance { + bestDistance = distance + } + + cost := 0.0 + for k, j := range start { + cost += referenceframe.InputsL2Distance(j, c.inputs[k]) + } + + best = append(best, entry{&c, distance, cost}) } - - cost := 0.0 - for k, j := range start { - cost += referenceframe.InputsL2Distance(j, c.inputs[k]) - } - - best = append(best, entry{&c, distance, cost}) } if len(best) == 0 { @@ -96,12 +163,13 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start return best[i].cost < best[j].cost }) - - for i := 0; i < len(best) && i < 5; i++ { - e := best[i] - logger.Debugf("%v dist: %02.f cost: %0.2f", e.e.inputs, e.distance, e.cost) + if false { + for i := 0; i < len(best) && i < 5; i++ { + e := best[i] + logger.Debugf("%v dist: %02.f cost: %0.2f", e.e.inputs, e.distance, e.cost) + } } - + return best[0].e.inputs, nil } @@ -113,10 +181,13 @@ func (ssc *smartSeedCache) distance(a, b referenceframe.FrameSystemPoses) float6 panic(fmt.Errorf("eliot fucked up %s", p.Parent())) } - pp := b[k] + pp, ok := b[k] + if !ok { + continue + } - if pp.Parent() != "world" { - panic(fmt.Errorf("eliot fucked up %s", pp.Parent())) + if pp == nil || pp.Parent() != "world" { + panic(fmt.Errorf("eliot fucked up %s", pp)) } dist += motionplan.WeightedSquaredNormDistance(p.Pose(), pp.Pose()) @@ -175,6 +246,8 @@ func (ssc *smartSeedCache) buildCache() error { return fmt.Errorf("cannot buildCache: %w", err) } + ssc.geoCache = map[string]*goalCache{} + for frame, _ := range ssc.rawCache[0].poses { err = ssc.buildInverseCache(frame) if err != nil { @@ -186,19 +259,40 @@ func (ssc *smartSeedCache) buildCache() error { } func (ssc *smartSeedCache) buildInverseCache(frame string) error { - var minCartesian, maxCartesian r3.Vector + gc := &goalCache{ + boxes: map[string]*goalCacheBox{}, + } for _, e := range ssc.rawCache { - minCartesian.X = min(minCartesian.X, e.poses[frame].Pose().Point().X) - minCartesian.Y = min(minCartesian.X, e.poses[frame].Pose().Point().Y) - minCartesian.Z = min(minCartesian.X, e.poses[frame].Pose().Point().X) + gc.minCartesian.X = min(gc.minCartesian.X, e.poses[frame].Pose().Point().X) + gc.minCartesian.Y = min(gc.minCartesian.X, e.poses[frame].Pose().Point().Y) + gc.minCartesian.Z = min(gc.minCartesian.X, e.poses[frame].Pose().Point().X) + + gc.maxCartesian.X = max(gc.maxCartesian.X, e.poses[frame].Pose().Point().X) + gc.maxCartesian.Y = max(gc.maxCartesian.X, e.poses[frame].Pose().Point().Y) + gc.maxCartesian.Z = max(gc.maxCartesian.X, e.poses[frame].Pose().Point().X) + } - maxCartesian.X = max(maxCartesian.X, e.poses[frame].Pose().Point().X) - maxCartesian.Y = max(maxCartesian.X, e.poses[frame].Pose().Point().Y) - maxCartesian.Z = max(maxCartesian.X, e.poses[frame].Pose().Point().X) + for _, e := range ssc.rawCache { + hash := gc.hash(e.poses[frame].Pose().Point()) + box, ok := gc.boxes[hash] + if !ok { + box = &goalCacheBox{hash: hash} + gc.boxes[hash] = box + } + box.entries = append(box.entries, e) } - fmt.Printf("%v -> %v %v\n", frame, minCartesian, maxCartesian) + for _, v := range gc.boxes { + for _, e := range v.entries { + p := e.poses[frame].Pose().Point() + v.center = v.center.Add(p) + } + + v.center = v.center.Mul( 1.0 / float64(len(v.entries)) ) + } + + ssc.geoCache[frame] = gc return nil @@ -226,12 +320,12 @@ func smartSeed(fs *referenceframe.FrameSystem) (*smartSeedCache, error) { lfs: lfs, } - + start := time.Now() err = c.buildCache() if err != nil { return nil, err } - + fmt.Printf("time to build: %v dof: %v\n", time.Since(start), len(lfs.dof)) foooooo[fs] = c return c, nil } diff --git a/motionplan/armplanning/smart_seed_test.go b/motionplan/armplanning/smart_seed_test.go index 1d2afd04d68..a368037b37b 100644 --- a/motionplan/armplanning/smart_seed_test.go +++ b/motionplan/armplanning/smart_seed_test.go @@ -2,7 +2,8 @@ package armplanning import ( "testing" - + "time" + "github.com/golang/geo/r3" "go.viam.com/rdk/logging" @@ -27,7 +28,8 @@ func TestSmartSeedCache1(t *testing.T) { test.That(t, err, test.ShouldBeNil) start := referenceframe.FrameSystemInputs{"ur5e" : {{1.0471667423817}, {0.011108350341463286}, {-1.0899013011625651}, {-3.0938870331059594}, {-1.767558957758243e-05}, {-3.681258507284093}}} - + + startTime := time.Now() seed, err := c.findSeed( referenceframe.FrameSystemPoses{"ur5e": referenceframe.NewPoseInFrame("world", spatialmath.NewPose( @@ -36,7 +38,8 @@ func TestSmartSeedCache1(t *testing.T) { ))}, start, logger) test.That(t, err, test.ShouldBeNil) - + logger.Infof("time to run findSeed: %v", time.Since(startTime)) + cost := referenceframe.InputsL2Distance(start["ur5e"], seed["ur5e"]) test.That(t, cost, test.ShouldBeLessThan, 1.25) diff --git a/motionplan/ik/combined.go b/motionplan/ik/combined.go index 48e910fb5ff..ea950b40bc5 100644 --- a/motionplan/ik/combined.go +++ b/motionplan/ik/combined.go @@ -68,14 +68,17 @@ func (ik *CombinedIK) Solve(ctx context.Context, parseed := rseed thisSolver := solver seedFloats := seed - if i > 1 { - seedFloats = generateRandomPositions(randSeed, lowerBound, upperBound) - } var myTravelPercent []float64 - if i == 0 { - // TODO: this is probablytoo conservative + if i <= len(ik.solvers) / 3 { + // TODO: this is probably too conservative + for _, p := range travelPercent { + myTravelPercent = append(myTravelPercent, max(.1, p)) + } + } else if i < (2 * len(ik.solvers) / 3) { myTravelPercent = travelPercent + } else { + seedFloats = generateRandomPositions(randSeed, lowerBound, upperBound) } activeSolvers.Add(1) From 9777fc584113368ba80c98bc8a6b5200a54ee4e9 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 15 Oct 2025 17:38:46 -0400 Subject: [PATCH 04/20] a mess --- motionplan/armplanning/smart_seed.go | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index 9f74ef58b4d..68c67b40af8 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -61,6 +61,13 @@ func (scs *smartSeedCache) findBoxes(goalFrame string, goalPose spatialmath.Pose d float64 } + if scs.geoCache[goalFrame] == nil { + err := scs.buildInverseCache(goalFrame) + if err != nil { + panic(err) + } + } + best := []e{} for _, b := range scs.geoCache[goalFrame].boxes { @@ -247,14 +254,14 @@ func (ssc *smartSeedCache) buildCache() error { } ssc.geoCache = map[string]*goalCache{} - + /* for frame, _ := range ssc.rawCache[0].poses { err = ssc.buildInverseCache(frame) if err != nil { return fmt.Errorf("cannot buildInverseCache for %s: %w", frame, err) } } - + */ return nil } From b69464479f899fa34ddc23519a2ad09e0be1fbe5 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 15 Oct 2025 17:56:56 -0400 Subject: [PATCH 05/20] lint --- .../armplanning/linearized_frame_system.go | 1 - motionplan/armplanning/node.go | 18 +-- motionplan/armplanning/real_test.go | 2 +- motionplan/armplanning/smart_seed.go | 109 ++++++++---------- motionplan/armplanning/smart_seed_test.go | 42 ++++--- motionplan/ik/combined.go | 2 +- motionplan/metrics.go | 1 - referenceframe/frame.go | 6 +- 8 files changed, 89 insertions(+), 92 deletions(-) diff --git a/motionplan/armplanning/linearized_frame_system.go b/motionplan/armplanning/linearized_frame_system.go index 43f6a6ee204..201a6642179 100644 --- a/motionplan/armplanning/linearized_frame_system.go +++ b/motionplan/armplanning/linearized_frame_system.go @@ -172,4 +172,3 @@ func (lfs *linearizedFrameSystem) jog(idx int, val, percentJog float64) float64 return val } - diff --git a/motionplan/armplanning/node.go b/motionplan/armplanning/node.go index 1a7de9412ee..47d4379a8af 100644 --- a/motionplan/armplanning/node.go +++ b/motionplan/armplanning/node.go @@ -142,19 +142,17 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er } if len(psc.pc.lfs.dof) <= 6 { - ssc, err := smartSeed(psc.pc.fs) + ssc, err := smartSeed(psc.pc.fs, psc.pc.logger) if err != nil { return nil, err } - + sss.seed, err = ssc.findSeed(psc.goal, psc.start, psc.pc.logger) if err != nil { return nil, err } } psc.pc.logger.Debugf("psc.start -> seed: \n%v\n%v", psc.start, sss.seed) - - sss.linearSeed, err = psc.pc.lfs.mapToSlice(sss.seed) if err != nil { return nil, err @@ -280,10 +278,8 @@ func (sss *solutionSolvingState) process(ctx context.Context, stepSolution *ik.S if len(sss.solutions) == 0 { sss.firstSolutionTime = time.Since(sss.startTime) - } - + } - myNode := &node{inputs: step, cost: sss.psc.pc.configurationDistanceFunc(stepArc)} sss.solutions = append(sss.solutions, myNode) @@ -303,7 +299,7 @@ func (sss *solutionSolvingState) process(ctx context.Context, stepSolution *ik.S // return bool is if we should stop because we're done. func (sss *solutionSolvingState) shouldStopEarly() bool { elapsed := time.Since(sss.startTime) - + if len(sss.solutions) >= sss.maxSolutions { sss.psc.pc.logger.Debugf("stopping with %d solutions after: %v", len(sss.solutions), elapsed) return true @@ -314,8 +310,7 @@ func (sss *solutionSolvingState) shouldStopEarly() bool { return true } - - if sss.bestScore < (sss.goodCost / 10) && elapsed > 100 * time.Millisecond { + if sss.bestScore < (sss.goodCost/10) && elapsed > 100*time.Millisecond { sss.psc.pc.logger.Debugf("stopping early with bestScore %0.2f (%0.2f) after: %v", sss.bestScore, sss.goodCost, elapsed) return true } @@ -325,7 +320,7 @@ func (sss *solutionSolvingState) shouldStopEarly() bool { multiple *= (sss.bestScore / sss.goodCost) } - if elapsed > max(sss.firstSolutionTime * time.Duration(multiple), 100*time.Millisecond) { + if elapsed > max(sss.firstSolutionTime*time.Duration(multiple), 100*time.Millisecond) { sss.psc.pc.logger.Debugf("stopping early with bestScore %0.2f after: %v", sss.bestScore, elapsed) return true } @@ -333,7 +328,6 @@ func (sss *solutionSolvingState) shouldStopEarly() bool { return false } - // getSolutions will initiate an IK solver for the given position and seed, collect solutions, and // score them by constraints. // diff --git a/motionplan/armplanning/real_test.go b/motionplan/armplanning/real_test.go index 61a17a5369f..7ea0749a8f2 100644 --- a/motionplan/armplanning/real_test.go +++ b/motionplan/armplanning/real_test.go @@ -188,7 +188,7 @@ func TestPirouette(t *testing.T) { fs := referenceframe.NewEmptyFrameSystem("pirouette") err = fs.AddFrame(armKinematics, fs.World()) test.That(t, err, test.ShouldBeNil) - + for iter := 0; iter < 10; iter++ { // keep track of previous index of idealJointValues, used for calculating expected joint 0 change prevIndex := 0 diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index 68c67b40af8..02fea52926b 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -6,7 +6,7 @@ import ( "time" "github.com/golang/geo/r3" - + "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" "go.viam.com/rdk/referenceframe" @@ -15,24 +15,24 @@ import ( type smartSeedCacheEntry struct { inputs referenceframe.FrameSystemInputs - poses referenceframe.FrameSystemPoses + poses referenceframe.FrameSystemPoses } type goalCacheBox struct { - hash string - center r3.Vector + hash string + center r3.Vector entries []smartSeedCacheEntry } type goalCache struct { minCartesian, maxCartesian r3.Vector - boxes map[string]*goalCacheBox // hash to list + boxes map[string]*goalCacheBox // hash to list } func (gc *goalCache) hashKey(value, min, max float64) int { x := (value - min) / (max - min) - d := int(x*10) + d := int(x * 10) if d >= 10 { d = 9 } @@ -47,7 +47,7 @@ func (gc *goalCache) hash(p r3.Vector) string { } type smartSeedCache struct { - fs *referenceframe.FrameSystem + fs *referenceframe.FrameSystem lfs *linearizedFrameSystem rawCache []smartSeedCacheEntry @@ -55,27 +55,24 @@ type smartSeedCache struct { geoCache map[string]*goalCache } -func (scs *smartSeedCache) findBoxes(goalFrame string, goalPose spatialmath.Pose) []*goalCacheBox { +func (ssc *smartSeedCache) findBoxes(goalFrame string, goalPose spatialmath.Pose) []*goalCacheBox { type e struct { b *goalCacheBox d float64 } - if scs.geoCache[goalFrame] == nil { - err := scs.buildInverseCache(goalFrame) - if err != nil { - panic(err) - } + if ssc.geoCache[goalFrame] == nil { + ssc.buildInverseCache(goalFrame) } - + best := []e{} - for _, b := range scs.geoCache[goalFrame].boxes { + for _, b := range ssc.geoCache[goalFrame].boxes { d := goalPose.Point().Distance(b.center) - best = append(best, e{b,d}) + best = append(best, e{b, d}) } - sort.Slice(best, func(a,b int) bool { + sort.Slice(best, func(a, b int) bool { return best[a].d < best[b].d }) @@ -84,37 +81,39 @@ func (scs *smartSeedCache) findBoxes(goalFrame string, goalPose spatialmath.Pose for i := 0; i < 10 && i < len(best); i++ { boxes = append(boxes, best[i].b) } - + return boxes } -func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start referenceframe.FrameSystemInputs, logger logging.Logger) (referenceframe.FrameSystemInputs, error) { +func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, + start referenceframe.FrameSystemInputs, + logger logging.Logger, +) (referenceframe.FrameSystemInputs, error) { if len(goal) > 1 { return nil, fmt.Errorf("smartSeedCache findSeed only works with 1 goal for now") } - + for _, p := range goal { - if p.Parent() != "world" { + if p.Parent() != referenceframe.World { return nil, fmt.Errorf("goal has to be in world, not %s", p.Parent()) } } goalFrame := "" var goalPose spatialmath.Pose - + for k, v := range goal { goalFrame = k goalPose = v.Pose() } type entry struct { - e *smartSeedCacheEntry + e *smartSeedCacheEntry distance float64 - cost float64 + cost float64 } - - best := []entry{} + best := []entry{} startPoses, err := start.ComputePoses(ssc.fs) if err != nil { @@ -122,7 +121,7 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start } startDistance := ssc.distance(startPoses, goal) - + bestDistance := startDistance * 2 boxes := ssc.findBoxes(goalFrame, goalPose) @@ -133,16 +132,16 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start if distance > (bestDistance * 2) { continue } - + if distance < bestDistance { bestDistance = distance } - + cost := 0.0 for k, j := range start { cost += referenceframe.InputsL2Distance(j, c.inputs[k]) } - + best = append(best, entry{&c, distance, cost}) } } @@ -151,8 +150,8 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start logger.Debugf("no best, returning start") return start, nil } - - sort.Slice(best, func(i,j int) bool { + + sort.Slice(best, func(i, j int) bool { return best[i].distance < best[j].distance }) @@ -166,7 +165,7 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start best = best[0:cutIdx] - sort.Slice(best, func(i,j int) bool { + sort.Slice(best, func(i, j int) bool { return best[i].cost < best[j].cost }) @@ -176,15 +175,15 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start logger.Debugf("%v dist: %02.f cost: %0.2f", e.e.inputs, e.distance, e.cost) } } - + return best[0].e.inputs, nil } func (ssc *smartSeedCache) distance(a, b referenceframe.FrameSystemPoses) float64 { dist := 0.0 - + for k, p := range a { - if p.Parent() != "world" { + if p.Parent() != referenceframe.World { panic(fmt.Errorf("eliot fucked up %s", p.Parent())) } @@ -192,8 +191,8 @@ func (ssc *smartSeedCache) distance(a, b referenceframe.FrameSystemPoses) float6 if !ok { continue } - - if pp == nil || pp.Parent() != "world" { + + if pp == nil || pp.Parent() != referenceframe.World { panic(fmt.Errorf("eliot fucked up %s", pp)) } @@ -214,11 +213,11 @@ func (ssc *smartSeedCache) addToCache(values []float64) error { } for _, p := range poses { - if p.Parent() != "world" { + if p.Parent() != referenceframe.World { return fmt.Errorf("why not in world, but %s", p.Parent()) } } - + ssc.rawCache = append(ssc.rawCache, smartSeedCacheEntry{inputs, poses}) return nil } @@ -234,10 +233,10 @@ func (ssc *smartSeedCache) buildRawCache(values []float64, joint int) error { min, max, r := ssc.lfs.dof[joint].GoodLimits() values[joint] = min - + jog := r / 10 for values[joint] <= max { - err := ssc.buildRawCache(values, joint + 1) + err := ssc.buildRawCache(values, joint+1) if err != nil { return err } @@ -255,17 +254,14 @@ func (ssc *smartSeedCache) buildCache() error { ssc.geoCache = map[string]*goalCache{} /* - for frame, _ := range ssc.rawCache[0].poses { - err = ssc.buildInverseCache(frame) - if err != nil { - return fmt.Errorf("cannot buildInverseCache for %s: %w", frame, err) + for frame := range ssc.rawCache[0].poses { + ssc.buildInverseCache(frame) } - } */ return nil } -func (ssc *smartSeedCache) buildInverseCache(frame string) error { +func (ssc *smartSeedCache) buildInverseCache(frame string) { gc := &goalCache{ boxes: map[string]*goalCacheBox{}, } @@ -296,34 +292,31 @@ func (ssc *smartSeedCache) buildInverseCache(frame string) error { v.center = v.center.Add(p) } - v.center = v.center.Mul( 1.0 / float64(len(v.entries)) ) + v.center = v.center.Mul(1.0 / float64(len(v.entries))) } - - ssc.geoCache[frame] = gc - - return nil + ssc.geoCache[frame] = gc } var foooooo map[*referenceframe.FrameSystem]*smartSeedCache -func smartSeed(fs *referenceframe.FrameSystem) (*smartSeedCache, error) { +func smartSeed(fs *referenceframe.FrameSystem, logger logging.Logger) (*smartSeedCache, error) { if foooooo == nil { foooooo = map[*referenceframe.FrameSystem]*smartSeedCache{} } c, ok := foooooo[fs] - fmt.Printf("ok: %v\n", ok) + logger.Warnf("ok: %v", ok) if ok { return c, nil } - + lfs, err := newLinearizedFrameSystem(fs) if err != nil { return nil, err } c = &smartSeedCache{ - fs: fs, + fs: fs, lfs: lfs, } @@ -332,7 +325,7 @@ func smartSeed(fs *referenceframe.FrameSystem) (*smartSeedCache, error) { if err != nil { return nil, err } - fmt.Printf("time to build: %v dof: %v\n", time.Since(start), len(lfs.dof)) + logger.Warnf("time to build: %v dof: %v", time.Since(start), len(lfs.dof)) foooooo[fs] = c return c, nil } diff --git a/motionplan/armplanning/smart_seed_test.go b/motionplan/armplanning/smart_seed_test.go index a368037b37b..e50185ab314 100644 --- a/motionplan/armplanning/smart_seed_test.go +++ b/motionplan/armplanning/smart_seed_test.go @@ -3,14 +3,14 @@ package armplanning import ( "testing" "time" - + "github.com/golang/geo/r3" - + "go.viam.com/test" + "go.viam.com/rdk/logging" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" - "go.viam.com/test" ) func TestSmartSeedCache1(t *testing.T) { @@ -24,30 +24,36 @@ func TestSmartSeedCache1(t *testing.T) { err = fs.AddFrame(armKinematics, fs.World()) test.That(t, err, test.ShouldBeNil) - c, err := smartSeed(fs) + c, err := smartSeed(fs, logger) test.That(t, err, test.ShouldBeNil) - start := referenceframe.FrameSystemInputs{"ur5e" : {{1.0471667423817}, {0.011108350341463286}, {-1.0899013011625651}, {-3.0938870331059594}, {-1.767558957758243e-05}, {-3.681258507284093}}} + start := referenceframe.FrameSystemInputs{"ur5e": { + {1.0471667423817}, + {0.011108350341463286}, + {-1.0899013011625651}, + {-3.0938870331059594}, + {-1.767558957758243e-05}, + {-3.681258507284093}, + }} startTime := time.Now() seed, err := c.findSeed( referenceframe.FrameSystemPoses{"ur5e": referenceframe.NewPoseInFrame("world", spatialmath.NewPose( - r3.Vector{X:-337.976430, Y:-464.051182, Z:554.695381}, - &spatialmath.OrientationVectorDegrees{OX:0.499987, OY:-0.866033, OZ:-0.000000, Theta:0.000000}, + r3.Vector{X: -337.976430, Y: -464.051182, Z: 554.695381}, + &spatialmath.OrientationVectorDegrees{OX: 0.499987, OY: -0.866033, OZ: -0.000000, Theta: 0.000000}, ))}, start, logger) test.That(t, err, test.ShouldBeNil) logger.Infof("time to run findSeed: %v", time.Since(startTime)) - + cost := referenceframe.InputsL2Distance(start["ur5e"], seed["ur5e"]) test.That(t, cost, test.ShouldBeLessThan, 1.25) - } func BenchmarkSmartSeedCacheSearch(t *testing.B) { logger := logging.NewTestLogger(t) - + armName := "ur5e" armKinematics, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/kinematics/ur5e.json"), armName) test.That(t, err, test.ShouldBeNil) @@ -56,10 +62,17 @@ func BenchmarkSmartSeedCacheSearch(t *testing.B) { err = fs.AddFrame(armKinematics, fs.World()) test.That(t, err, test.ShouldBeNil) - c, err := smartSeed(fs) + c, err := smartSeed(fs, logger) test.That(t, err, test.ShouldBeNil) - start := referenceframe.FrameSystemInputs{"ur5e" : {{1.0471667423817}, {0.011108350341463286}, {-1.0899013011625651}, {-3.0938870331059594}, {-1.767558957758243e-05}, {-3.681258507284093}}} + start := referenceframe.FrameSystemInputs{"ur5e": { + {1.0471667423817}, + {0.011108350341463286}, + {-1.0899013011625651}, + {-3.0938870331059594}, + {-1.767558957758243e-05}, + {-3.681258507284093}, + }} t.ResetTimer() @@ -67,11 +80,10 @@ func BenchmarkSmartSeedCacheSearch(t *testing.B) { _, err = c.findSeed( referenceframe.FrameSystemPoses{"ur5e": referenceframe.NewPoseInFrame("world", spatialmath.NewPose( - r3.Vector{X:-337.976430, Y:-464.051182, Z:554.695381}, - &spatialmath.OrientationVectorDegrees{OX:0.499987, OY:-0.866033, OZ:-0.000000, Theta:0.000000}, + r3.Vector{X: -337.976430, Y: -464.051182, Z: 554.695381}, + &spatialmath.OrientationVectorDegrees{OX: 0.499987, OY: -0.866033, OZ: -0.000000, Theta: 0.000000}, ))}, start, logger) test.That(t, err, test.ShouldBeNil) } - } diff --git a/motionplan/ik/combined.go b/motionplan/ik/combined.go index ea950b40bc5..28bdd6b5f91 100644 --- a/motionplan/ik/combined.go +++ b/motionplan/ik/combined.go @@ -70,7 +70,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, seedFloats := seed var myTravelPercent []float64 - if i <= len(ik.solvers) / 3 { + if i <= len(ik.solvers)/3 { // TODO: this is probably too conservative for _, p := range travelPercent { myTravelPercent = append(myTravelPercent, max(.1, p)) diff --git a/motionplan/metrics.go b/motionplan/metrics.go index e9a8ad32d3e..92d333cb8d9 100644 --- a/motionplan/metrics.go +++ b/motionplan/metrics.go @@ -312,7 +312,6 @@ func WeightedSquaredNormDistance(start, end spatial.Pose) float64 { return ptDelta + orientDelta } - // TODO(RSDK-2557): Writing a PenetrationDepthMetric will allow cbirrt to path along the sides of obstacles rather than terminating // the RRT tree when an obstacle is hit diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 87a2f6b3cda..3c5012010f4 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -37,17 +37,17 @@ func (l *Limit) Range() float64 { const rangeLimit = 999 -// GoodLimits gives min, max, range, but capped to -999,999 +// GoodLimits gives min, max, range, but capped to -999,999. func (l *Limit) GoodLimits() (float64, float64, float64) { a := l.Min b := l.Max - if a < -1 * rangeLimit { + if a < -1*rangeLimit { a = -1 * rangeLimit } if b > rangeLimit { b = rangeLimit } - return a, b, b-a + return a, b, b - a } func limitsAlmostEqual(limits1, limits2 []Limit, epsilon float64) bool { From 230bfffb9bdc414096c08807a6b53931909314de Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 15 Oct 2025 22:22:48 -0400 Subject: [PATCH 06/20] turn off smarts --- motionplan/armplanning/node.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motionplan/armplanning/node.go b/motionplan/armplanning/node.go index 47d4379a8af..70c35b6d889 100644 --- a/motionplan/armplanning/node.go +++ b/motionplan/armplanning/node.go @@ -141,7 +141,7 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er sss.maxSolutions = defaultSolutionsToSeed } - if len(psc.pc.lfs.dof) <= 6 { + if len(psc.pc.lfs.dof) <= 0 { ssc, err := smartSeed(psc.pc.fs, psc.pc.logger) if err != nil { return nil, err From 1b96aba59eeb557d4c8ed3c9341368bac092efac Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Sun, 19 Oct 2025 09:41:31 -0400 Subject: [PATCH 07/20] before merge conflict --- motionplan/armplanning/node.go | 86 ++++++++++++++++------------- motionplan/armplanning/real_test.go | 2 - motionplan/ik/combined.go | 19 +++++-- 3 files changed, 61 insertions(+), 46 deletions(-) diff --git a/motionplan/armplanning/node.go b/motionplan/armplanning/node.go index 9fa762f8a83..a8306da5b2c 100644 --- a/motionplan/armplanning/node.go +++ b/motionplan/armplanning/node.go @@ -119,39 +119,31 @@ type solutionSolvingState struct { solutions []*node startTime time.Time - bestScore float64 firstSolutionTime time.Duration + + bestScoreWithProblem float64 + bestScoreNoProblem float64 } func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, error) { var err error sss := &solutionSolvingState{ - psc: psc, - seed: psc.start, - solutions: []*node{}, - failures: newIkConstraintError(psc.pc.fs, psc.checker), - startTime: time.Now(), - firstSolutionTime: time.Hour, - bestScore: 10000000, - maxSolutions: psc.pc.planOpts.MaxSolutions, + psc: psc, + seed: psc.start, + solutions: []*node{}, + failures: newIkConstraintError(psc.pc.fs, psc.checker), + startTime: time.Now(), + firstSolutionTime: time.Hour, + bestScoreNoProblem: 10000000, + bestScoreWithProblem: 10000000, + maxSolutions: psc.pc.planOpts.MaxSolutions, } if sss.maxSolutions <= 0 { sss.maxSolutions = defaultSolutionsToSeed } - if len(psc.pc.lfs.dof) <= 0 { - ssc, err := smartSeed(psc.pc.fs, psc.pc.logger) - if err != nil { - return nil, err - } - - sss.seed, err = ssc.findSeed(psc.goal, psc.start, psc.pc.logger) - if err != nil { - return nil, err - } - } psc.pc.logger.Debugf("psc.start -> seed: \n%v\n%v", psc.start, sss.seed) sss.linearSeed, err = psc.pc.lfs.mapToSlice(sss.seed) if err != nil { @@ -229,8 +221,7 @@ func (sss *solutionSolvingState) nonchainMinimize(ctx context.Context, } // return bool is if we should stop because we're done. -func (sss *solutionSolvingState) process(ctx context.Context, stepSolution *ik.Solution, -) bool { +func (sss *solutionSolvingState) process(ctx context.Context, stepSolution *ik.Solution) bool { ctx, span := trace.StartSpan(ctx, "process") defer span.End() sss.processCalls++ @@ -286,14 +277,18 @@ func (sss *solutionSolvingState) process(ctx context.Context, stepSolution *ik.S myNode := &node{inputs: step, cost: sss.psc.pc.configurationDistanceFunc(stepArc)} sss.solutions = append(sss.solutions, myNode) - if myNode.cost < sss.bestScore { - sss.bestScore = myNode.cost + if myNode.cost < sss.bestScoreWithProblem { + sss.bestScoreWithProblem = myNode.cost } - if myNode.cost < min(sss.goodCost, sss.bestScore*defaultOptimalityMultiple) { + if myNode.cost < min(sss.goodCost, sss.bestScoreWithProblem*defaultOptimalityMultiple) { whyNot := sss.psc.checkPath(ctx, sss.psc.start, step) sss.psc.pc.logger.Debugf("got score %0.4f and goodCost: %0.2f - result: %v", myNode.cost, sss.goodCost, whyNot) myNode.checkPath = whyNot == nil + + if whyNot == nil && myNode.cost < sss.bestScoreNoProblem { + sss.bestScoreNoProblem = myNode.cost + } } return sss.shouldStopEarly() @@ -308,23 +303,36 @@ func (sss *solutionSolvingState) shouldStopEarly() bool { return true } - if sss.bestScore < .2 { - sss.psc.pc.logger.Debugf("stopping early with amazing %0.2f after: %v", sss.bestScore, elapsed) + if sss.bestScoreNoProblem < .2 { + sss.psc.pc.logger.Debugf("stopping early with amazing %0.2f after: %v", sss.bestScoreNoProblem, elapsed) return true } - if sss.bestScore < (sss.goodCost/10) && elapsed > 100*time.Millisecond { - sss.psc.pc.logger.Debugf("stopping early with bestScore %0.2f (%0.2f) after: %v", sss.bestScore, sss.goodCost, elapsed) - return true - } - - multiple := 50.0 - if sss.bestScore < sss.goodCost { - multiple *= (sss.bestScore / sss.goodCost) - } - - if elapsed > max(sss.firstSolutionTime*time.Duration(multiple), 100*time.Millisecond) { - sss.psc.pc.logger.Debugf("stopping early with bestScore %0.2f after: %v", sss.bestScore, elapsed) + multiple := 100.0 + minMillis := 250 + + if sss.bestScoreNoProblem < sss.goodCost/20 { + multiple = 0 + minMillis = 10 + } else if sss.bestScoreNoProblem < sss.goodCost/10 { + multiple = 0 + minMillis = 20 + } else if sss.bestScoreNoProblem < sss.goodCost/5 { + multiple = 40 + minMillis = 125 + } else if sss.bestScoreNoProblem < sss.goodCost/2 { + multiple = 40 + minMillis = 150 + } else if sss.bestScoreNoProblem < sss.goodCost { + multiple = 50 + } else if sss.bestScoreWithProblem < sss.goodCost { + // we're going to have to do cbirrt, so look a little less, but still look + multiple = 75 + } + + if elapsed > max(sss.firstSolutionTime*time.Duration(multiple), time.Duration(minMillis)*time.Millisecond) { + sss.psc.pc.logger.Debugf("stopping early with bestScore %0.2f / %0.2f after: %v", + sss.bestScoreNoProblem, sss.bestScoreWithProblem, elapsed) return true } diff --git a/motionplan/armplanning/real_test.go b/motionplan/armplanning/real_test.go index 7ea0749a8f2..0c37ddabbbb 100644 --- a/motionplan/armplanning/real_test.go +++ b/motionplan/armplanning/real_test.go @@ -148,8 +148,6 @@ func TestSandingLargeMove1(t *testing.T) { } func TestPirouette(t *testing.T) { - // t.Skip() - // get arm kinematics for forward kinematics armName := "ur5e" armKinematics, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/kinematics/ur5e.json"), armName) diff --git a/motionplan/ik/combined.go b/motionplan/ik/combined.go index 66e251d901d..369669cda33 100644 --- a/motionplan/ik/combined.go +++ b/motionplan/ik/combined.go @@ -30,7 +30,9 @@ func CreateCombinedIKSolver( ) (*CombinedIK, error) { ik := &CombinedIK{} ik.limits = limits - nCPU = max(nCPU, 2) + nCPU = max(nCPU, 4) + + logger.Debugf("CreateCombinedIKSolver nCPU: %d", nCPU) for i := 1; i <= nCPU; i++ { nloptSolver, err := CreateNloptSolver(ik.limits, logger, -1, true, true) @@ -70,12 +72,11 @@ func (ik *CombinedIK) Solve(ctx context.Context, seedFloats := seed var myTravelPercent []float64 - if i <= len(ik.solvers)/3 { - // TODO: this is probably too conservative + if bottomThird(i, len(ik.solvers)) { for _, p := range travelPercent { - myTravelPercent = append(myTravelPercent, max(.1, p)) + myTravelPercent = append(myTravelPercent, max(.2, p)) } - } else if i < (2 * len(ik.solvers) / 3) { + } else if middleThird(i, len(ik.solvers)) { myTravelPercent = travelPercent } else { seedFloats = generateRandomPositions(randSeed, lowerBound, upperBound) @@ -102,3 +103,11 @@ func (ik *CombinedIK) Solve(ctx context.Context, func (ik *CombinedIK) DoF() []referenceframe.Limit { return ik.limits } + +func bottomThird(i, l int) bool { + return i <= l/3 +} + +func middleThird(i, l int) bool { + return i <= ((2 * l) / 3) +} From 774894c7dc343fc35ee7d60dac3e5bc99db7e7f8 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Sun, 19 Oct 2025 09:46:58 -0400 Subject: [PATCH 08/20] put back --- motionplan/armplanning/node.go | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/motionplan/armplanning/node.go b/motionplan/armplanning/node.go index a8306da5b2c..5e28a7926be 100644 --- a/motionplan/armplanning/node.go +++ b/motionplan/armplanning/node.go @@ -144,6 +144,18 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er sss.maxSolutions = defaultSolutionsToSeed } + if len(psc.pc.lfs.dof) <= 0 { + ssc, err := smartSeed(psc.pc.fs, psc.pc.logger) + if err != nil { + return nil, err + } + + sss.seed, err = ssc.findSeed(psc.goal, psc.start, psc.pc.logger) + if err != nil { + return nil, err + } + } + psc.pc.logger.Debugf("psc.start -> seed: \n%v\n%v", psc.start, sss.seed) sss.linearSeed, err = psc.pc.lfs.mapToSlice(sss.seed) if err != nil { From c372fce3f521ef7ab8c230b6a388f7a844aed867 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Mon, 20 Oct 2025 02:07:24 -0400 Subject: [PATCH 09/20] broken wip --- motionplan/armplanning/smart_seed.go | 39 ++++++++++---- motionplan/armplanning/smart_seed_test.go | 66 +++++++++++++++++++++++ 2 files changed, 94 insertions(+), 11 deletions(-) diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index 02fea52926b..44e12169727 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -78,7 +78,7 @@ func (ssc *smartSeedCache) findBoxes(goalFrame string, goalPose spatialmath.Pose boxes := []*goalCacheBox{} - for i := 0; i < 10 && i < len(best); i++ { + for i := 0; i < 100000 && i < len(best); i++ { boxes = append(boxes, best[i].b) } @@ -89,6 +89,21 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start referenceframe.FrameSystemInputs, logger logging.Logger, ) (referenceframe.FrameSystemInputs, error) { + ss, err := ssc.findSeeds(goal, start, logger) + if err != nil { + return nil, err + } + if len(ss) == 0 { + return nil, fmt.Errorf("no findSeeds results") + } + return ss[0], nil +} + +func (ssc *smartSeedCache) findSeeds(goal referenceframe.FrameSystemPoses, + start referenceframe.FrameSystemInputs, + logger logging.Logger, +) ([]referenceframe.FrameSystemInputs, error) { + if len(goal) > 1 { return nil, fmt.Errorf("smartSeedCache findSeed only works with 1 goal for now") } @@ -121,7 +136,7 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, } startDistance := ssc.distance(startPoses, goal) - + logger.Debugf("startPoses: %v", startDistance) bestDistance := startDistance * 2 boxes := ssc.findBoxes(goalFrame, goalPose) @@ -129,6 +144,7 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, for _, b := range boxes { for _, c := range b.entries { distance := ssc.distance(goal, c.poses) + logger.Debugf("\t distance: %v", distance) if distance > (bestDistance * 2) { continue } @@ -148,7 +164,7 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, if len(best) == 0 { logger.Debugf("no best, returning start") - return start, nil + return []referenceframe.FrameSystemInputs{start}, nil } sort.Slice(best, func(i, j int) bool { @@ -158,7 +174,7 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, cutIdx := 0 for cutIdx < len(best) { if best[cutIdx].distance > (2 * best[0].distance) { - break + //break } cutIdx++ } @@ -169,14 +185,15 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, return best[i].cost < best[j].cost }) - if false { - for i := 0; i < len(best) && i < 5; i++ { - e := best[i] - logger.Debugf("%v dist: %02.f cost: %0.2f", e.e.inputs, e.distance, e.cost) - } + ret := []referenceframe.FrameSystemInputs{} + + for i := 0; i < len(best) /*&& i < 5*/; i++ { + e := best[i] + ret = append(ret, e.e.inputs) + //logger.Debugf("%v dist: %02.f cost: %0.2f", e.e.inputs, e.distance, e.cost) } - return best[0].e.inputs, nil + return ret, nil } func (ssc *smartSeedCache) distance(a, b referenceframe.FrameSystemPoses) float64 { @@ -234,7 +251,7 @@ func (ssc *smartSeedCache) buildRawCache(values []float64, joint int) error { min, max, r := ssc.lfs.dof[joint].GoodLimits() values[joint] = min - jog := r / 10 + jog := r / 12 for values[joint] <= max { err := ssc.buildRawCache(values, joint+1) if err != nil { diff --git a/motionplan/armplanning/smart_seed_test.go b/motionplan/armplanning/smart_seed_test.go index e50185ab314..639df006294 100644 --- a/motionplan/armplanning/smart_seed_test.go +++ b/motionplan/armplanning/smart_seed_test.go @@ -51,6 +51,72 @@ func TestSmartSeedCache1(t *testing.T) { test.That(t, cost, test.ShouldBeLessThan, 1.25) } +func TestSmartSeedCachePirouette(t *testing.T) { + logger := logging.NewTestLogger(t) + + armName := "ur5e" + armKinematics, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/kinematics/ur5e.json"), armName) + test.That(t, err, test.ShouldBeNil) + + idealJointValues := [][]referenceframe.Input{ + {{0 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{30 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{60 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{90 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{120 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{150 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{180 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{180 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{150 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{120 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{90 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{60 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{30 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{0 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + } + + fs := referenceframe.NewEmptyFrameSystem("pirouette") + err = fs.AddFrame(armKinematics, fs.World()) + test.That(t, err, test.ShouldBeNil) + + ssc, err := smartSeed(fs, logging.NewTestLogger(t)) + test.That(t, err, test.ShouldBeNil) + + for i, ideal := range idealJointValues { + pose, err := armKinematics.Transform(ideal) + test.That(t, err, test.ShouldBeNil) + + score1 := referenceframe.InputsL2Distance(idealJointValues[0], ideal) + logger.Infof("hi1 %v", score1) + seeds, err := ssc.findSeeds( + referenceframe.FrameSystemPoses{armName:referenceframe.NewPoseInFrame("world", pose)}, + referenceframe.FrameSystemInputs{armName: idealJointValues[0]}, + logger) + test.That(t, err, test.ShouldBeNil) + firstScore := 0.0 + for ii, seed := range seeds { + score2 := referenceframe.InputsL2Distance(seed[armName], idealJointValues[i]) + if ii == 0 { + firstScore = score2 + } + logger.Infof("\t %d %v", ii, score2) + if score2 < score1 { + break + } + if score1 == 0 { + break + } + } + + if score1 > 0 { + test.That(t, firstScore, test.ShouldBeLessThan, score1) + } + + } + + t.Fail() +} + func BenchmarkSmartSeedCacheSearch(t *testing.B) { logger := logging.NewTestLogger(t) From 01d7ea7ea625448a673eaeb03ad97c18edba879e Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Mon, 20 Oct 2025 15:13:21 -0400 Subject: [PATCH 10/20] better --- motionplan/armplanning/node.go | 5 ++- motionplan/armplanning/real_test.go | 35 +++++++++++--------- motionplan/armplanning/smart_seed.go | 40 +++++++++++++---------- motionplan/armplanning/smart_seed_test.go | 35 ++++++-------------- 4 files changed, 56 insertions(+), 59 deletions(-) diff --git a/motionplan/armplanning/node.go b/motionplan/armplanning/node.go index 5e28a7926be..d6d6ab1d737 100644 --- a/motionplan/armplanning/node.go +++ b/motionplan/armplanning/node.go @@ -144,7 +144,7 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er sss.maxSolutions = defaultSolutionsToSeed } - if len(psc.pc.lfs.dof) <= 0 { + if len(psc.pc.lfs.dof) <= 6 { ssc, err := smartSeed(psc.pc.fs, psc.pc.logger) if err != nil { return nil, err @@ -326,6 +326,9 @@ func (sss *solutionSolvingState) shouldStopEarly() bool { if sss.bestScoreNoProblem < sss.goodCost/20 { multiple = 0 minMillis = 10 + } else if sss.bestScoreNoProblem < sss.goodCost/15 { + multiple = 1 + minMillis = 15 } else if sss.bestScoreNoProblem < sss.goodCost/10 { multiple = 0 minMillis = 20 diff --git a/motionplan/armplanning/real_test.go b/motionplan/armplanning/real_test.go index 0c37ddabbbb..54aaa223929 100644 --- a/motionplan/armplanning/real_test.go +++ b/motionplan/armplanning/real_test.go @@ -147,28 +147,31 @@ func TestSandingLargeMove1(t *testing.T) { test.That(t, len(solution.steps), test.ShouldEqual, 1) } +var pirIdealJointValues = [][]referenceframe.Input{ + {{0 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{30 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{60 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{90 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{120 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{150 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{180 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{180 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{150 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{120 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{90 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{60 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{30 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, + {{0 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, +} + func TestPirouette(t *testing.T) { // get arm kinematics for forward kinematics armName := "ur5e" armKinematics, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/kinematics/ur5e.json"), armName) test.That(t, err, test.ShouldBeNil) - idealJointValues := [][]referenceframe.Input{ - {{0 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{30 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{60 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{90 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{120 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{150 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{180 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{180 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{150 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{120 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{90 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{60 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{30 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{0 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - } + idealJointValues := pirIdealJointValues + // the only change here is in joint 0 in increments of 30, while all the other joints are kept at a constant value // below is change in joint 0 in degrees: // 0 -> 30 -> 60 -> 90 -> 120 -> 150 -> 180 -> 180 -> 150 -> 120 -> 90 -> 60 -> 30 -> 0 diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index 44e12169727..51394061759 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -32,18 +32,14 @@ type goalCache struct { func (gc *goalCache) hashKey(value, min, max float64) int { x := (value - min) / (max - min) - d := int(x * 10) - if d >= 10 { - d = 9 - } - return d + return int(x * 100) } func (gc *goalCache) hash(p r3.Vector) string { x := gc.hashKey(p.X, gc.minCartesian.X, gc.maxCartesian.X) y := gc.hashKey(p.Y, gc.minCartesian.Y, gc.maxCartesian.Y) z := gc.hashKey(p.Z, gc.minCartesian.Z, gc.maxCartesian.Z) - return fmt.Sprintf("%d%d%d", x, y, z) + return fmt.Sprintf("%0.3d%0.3d%0.3d", x, y, z) } type smartSeedCache struct { @@ -78,7 +74,7 @@ func (ssc *smartSeedCache) findBoxes(goalFrame string, goalPose spatialmath.Pose boxes := []*goalCacheBox{} - for i := 0; i < 100000 && i < len(best); i++ { + for i := 0; i < 100 && i < len(best); i++ { boxes = append(boxes, best[i].b) } @@ -103,7 +99,6 @@ func (ssc *smartSeedCache) findSeeds(goal referenceframe.FrameSystemPoses, start referenceframe.FrameSystemInputs, logger logging.Logger, ) ([]referenceframe.FrameSystemInputs, error) { - if len(goal) > 1 { return nil, fmt.Errorf("smartSeedCache findSeed only works with 1 goal for now") } @@ -135,8 +130,8 @@ func (ssc *smartSeedCache) findSeeds(goal referenceframe.FrameSystemPoses, return nil, err } - startDistance := ssc.distance(startPoses, goal) - logger.Debugf("startPoses: %v", startDistance) + startDistance := max(1, ssc.distance(startPoses, goal)) + logger.Debugf("startDistance: %v", startDistance) bestDistance := startDistance * 2 boxes := ssc.findBoxes(goalFrame, goalPose) @@ -144,7 +139,6 @@ func (ssc *smartSeedCache) findSeeds(goal referenceframe.FrameSystemPoses, for _, b := range boxes { for _, c := range b.entries { distance := ssc.distance(goal, c.poses) - logger.Debugf("\t distance: %v", distance) if distance > (bestDistance * 2) { continue } @@ -174,7 +168,7 @@ func (ssc *smartSeedCache) findSeeds(goal referenceframe.FrameSystemPoses, cutIdx := 0 for cutIdx < len(best) { if best[cutIdx].distance > (2 * best[0].distance) { - //break + break } cutIdx++ } @@ -186,11 +180,11 @@ func (ssc *smartSeedCache) findSeeds(goal referenceframe.FrameSystemPoses, }) ret := []referenceframe.FrameSystemInputs{} - - for i := 0; i < len(best) /*&& i < 5*/; i++ { + + for i := 0; i < len(best) && i < 5; i++ { e := best[i] ret = append(ret, e.e.inputs) - //logger.Debugf("%v dist: %02.f cost: %0.2f", e.e.inputs, e.distance, e.cost) + // logger.Debugf("%v dist: %02.f cost: %0.2f", e.e.inputs, e.distance, e.cost) } return ret, nil @@ -251,14 +245,16 @@ func (ssc *smartSeedCache) buildRawCache(values []float64, joint int) error { min, max, r := ssc.lfs.dof[joint].GoodLimits() values[joint] = min - jog := r / 12 + jog := r / 10 for values[joint] <= max { err := ssc.buildRawCache(values, joint+1) if err != nil { return err } + values[joint] += jog } + return nil } @@ -269,6 +265,16 @@ func (ssc *smartSeedCache) buildCache() error { return fmt.Errorf("cannot buildCache: %w", err) } + /* + comps := 0 + for i1, _ := range ssc.rawCache { + for i2 := i1 + 1; i2 < len(ssc.rawCache); i2++ { + comps++ + } + } + + fmt.Printf("comps: %v\n", comps) + */ ssc.geoCache = map[string]*goalCache{} /* for frame := range ssc.rawCache[0].poses { @@ -342,7 +348,7 @@ func smartSeed(fs *referenceframe.FrameSystem, logger logging.Logger) (*smartSee if err != nil { return nil, err } - logger.Warnf("time to build: %v dof: %v", time.Since(start), len(lfs.dof)) + logger.Warnf("time to build: %v dof: %v rawCache size: %d", time.Since(start), len(lfs.dof), len(c.rawCache)) foooooo[fs] = c return c, nil } diff --git a/motionplan/armplanning/smart_seed_test.go b/motionplan/armplanning/smart_seed_test.go index 639df006294..8a6f320c5d1 100644 --- a/motionplan/armplanning/smart_seed_test.go +++ b/motionplan/armplanning/smart_seed_test.go @@ -53,43 +53,28 @@ func TestSmartSeedCache1(t *testing.T) { func TestSmartSeedCachePirouette(t *testing.T) { logger := logging.NewTestLogger(t) - + armName := "ur5e" armKinematics, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/kinematics/ur5e.json"), armName) test.That(t, err, test.ShouldBeNil) - idealJointValues := [][]referenceframe.Input{ - {{0 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{30 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{60 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{90 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{120 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{150 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{180 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{180 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{150 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{120 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{90 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{60 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{30 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - {{0 * 3.1415 / 180.0}, {0}, {-90 * 3.1415 / 180.0}, {0}, {0}, {0}}, - } + idealJointValues := pirIdealJointValues fs := referenceframe.NewEmptyFrameSystem("pirouette") err = fs.AddFrame(armKinematics, fs.World()) test.That(t, err, test.ShouldBeNil) - + ssc, err := smartSeed(fs, logging.NewTestLogger(t)) test.That(t, err, test.ShouldBeNil) - + for i, ideal := range idealJointValues { pose, err := armKinematics.Transform(ideal) test.That(t, err, test.ShouldBeNil) score1 := referenceframe.InputsL2Distance(idealJointValues[0], ideal) - logger.Infof("hi1 %v", score1) + logger.Infof("hi %d %v", i, score1) seeds, err := ssc.findSeeds( - referenceframe.FrameSystemPoses{armName:referenceframe.NewPoseInFrame("world", pose)}, + referenceframe.FrameSystemPoses{armName: referenceframe.NewPoseInFrame("world", pose)}, referenceframe.FrameSystemInputs{armName: idealJointValues[0]}, logger) test.That(t, err, test.ShouldBeNil) @@ -106,15 +91,15 @@ func TestSmartSeedCachePirouette(t *testing.T) { if score1 == 0 { break } + if ii > 10 { + break + } } if score1 > 0 { - test.That(t, firstScore, test.ShouldBeLessThan, score1) + test.That(t, firstScore, test.ShouldBeLessThan, 4) } - } - - t.Fail() } func BenchmarkSmartSeedCacheSearch(t *testing.B) { From c1bd866614ce8614e94cf58c49831b39eefcb1f5 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Tue, 21 Oct 2025 15:11:47 -0400 Subject: [PATCH 11/20] change name --- motionplan/armplanning/smart_seed.go | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index 51394061759..c0c93856c60 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -19,7 +19,7 @@ type smartSeedCacheEntry struct { } type goalCacheBox struct { - hash string + boxKey string center r3.Vector entries []smartSeedCacheEntry } @@ -29,16 +29,16 @@ type goalCache struct { boxes map[string]*goalCacheBox // hash to list } -func (gc *goalCache) hashKey(value, min, max float64) int { +func (gc *goalCache) boxKeyCompute(value, min, max float64) int { x := (value - min) / (max - min) return int(x * 100) } -func (gc *goalCache) hash(p r3.Vector) string { - x := gc.hashKey(p.X, gc.minCartesian.X, gc.maxCartesian.X) - y := gc.hashKey(p.Y, gc.minCartesian.Y, gc.maxCartesian.Y) - z := gc.hashKey(p.Z, gc.minCartesian.Z, gc.maxCartesian.Z) +func (gc *goalCache) boxKey(p r3.Vector) string { + x := gc.boxKeyCompute(p.X, gc.minCartesian.X, gc.maxCartesian.X) + y := gc.boxKeyCompute(p.Y, gc.minCartesian.Y, gc.maxCartesian.Y) + z := gc.boxKeyCompute(p.Z, gc.minCartesian.Z, gc.maxCartesian.Z) return fmt.Sprintf("%0.3d%0.3d%0.3d", x, y, z) } @@ -300,11 +300,11 @@ func (ssc *smartSeedCache) buildInverseCache(frame string) { } for _, e := range ssc.rawCache { - hash := gc.hash(e.poses[frame].Pose().Point()) - box, ok := gc.boxes[hash] + key := gc.boxKey(e.poses[frame].Pose().Point()) + box, ok := gc.boxes[key] if !ok { - box = &goalCacheBox{hash: hash} - gc.boxes[hash] = box + box = &goalCacheBox{boxKey: key} + gc.boxes[key] = box } box.entries = append(box.entries, e) } From 3fbfceef92c11b5aaf34cebfa7252de572d53a55 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 22 Oct 2025 09:13:04 -0400 Subject: [PATCH 12/20] compiles --- motionplan/armplanning/smart_seed.go | 48 ++++++++++----------- motionplan/tpspace/ptgGroupFrame.go | 31 ++++++++++++++ motionplan/tpspace/ptgIKFrame.go | 15 +++++++ pointcloud/basic_octree.go | 21 ++++++++++ referenceframe/frame.go | 56 +++++++++++++++++++++++++ referenceframe/hash.go | 21 ++++++++++ referenceframe/input.go | 5 +++ referenceframe/model.go | 8 ++++ referenceframe/register_test.go | 4 ++ services/motion/builtin/wrapperFrame.go | 4 ++ spatialmath/axisAngle.go | 10 +++++ spatialmath/box.go | 4 ++ spatialmath/capsule.go | 9 ++++ spatialmath/dualquaternion.go | 5 +++ spatialmath/eulerangles.go | 9 ++++ spatialmath/geometry.go | 2 + spatialmath/mesh.go | 15 +++++++ spatialmath/orientationVector.go | 20 +++++++++ spatialmath/point.go | 19 +++++++++ spatialmath/pose.go | 23 ++++++++++ spatialmath/quaternion.go | 10 +++++ spatialmath/rotationMatrix.go | 9 ++++ spatialmath/sphere.go | 4 ++ spatialmath/triangle.go | 15 +++++++ 24 files changed, 342 insertions(+), 25 deletions(-) create mode 100644 referenceframe/hash.go diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index c0c93856c60..9fa16377511 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -3,6 +3,7 @@ package armplanning import ( "fmt" "sort" + "sync" "time" "github.com/golang/geo/r3" @@ -258,29 +259,17 @@ func (ssc *smartSeedCache) buildRawCache(values []float64, joint int) error { return nil } -func (ssc *smartSeedCache) buildCache() error { +func (ssc *smartSeedCache) buildCache(logger logging.Logger) error { + logger.Debugf("buildCache %v", ssc.lfs.dof) + values := make([]float64, len(ssc.lfs.dof)) err := ssc.buildRawCache(values, 0) if err != nil { return fmt.Errorf("cannot buildCache: %w", err) } - /* - comps := 0 - for i1, _ := range ssc.rawCache { - for i2 := i1 + 1; i2 < len(ssc.rawCache); i2++ { - comps++ - } - } - - fmt.Printf("comps: %v\n", comps) - */ ssc.geoCache = map[string]*goalCache{} - /* - for frame := range ssc.rawCache[0].poses { - ssc.buildInverseCache(frame) - } - */ + return nil } @@ -321,14 +310,19 @@ func (ssc *smartSeedCache) buildInverseCache(frame string) { ssc.geoCache[frame] = gc } -var foooooo map[*referenceframe.FrameSystem]*smartSeedCache +var ( + sscCache map[int]*smartSeedCache = map[int]*smartSeedCache{} + sscCacheLock sync.Mutex +) func smartSeed(fs *referenceframe.FrameSystem, logger logging.Logger) (*smartSeedCache, error) { - if foooooo == nil { - foooooo = map[*referenceframe.FrameSystem]*smartSeedCache{} - } - c, ok := foooooo[fs] - logger.Warnf("ok: %v", ok) + hash := fs.Hash() + var c *smartSeedCache + + sscCacheLock.Lock() + c, ok := sscCache[hash] + sscCacheLock.Unlock() + if ok { return c, nil } @@ -344,11 +338,15 @@ func smartSeed(fs *referenceframe.FrameSystem, logger logging.Logger) (*smartSee } start := time.Now() - err = c.buildCache() + err = c.buildCache(logger) if err != nil { return nil, err } - logger.Warnf("time to build: %v dof: %v rawCache size: %d", time.Since(start), len(lfs.dof), len(c.rawCache)) - foooooo[fs] = c + logger.Warnf("time to build: %v dof: %v rawCache size: %d hash: %v", time.Since(start), len(lfs.dof), len(c.rawCache), hash) + + sscCacheLock.Lock() + sscCache[hash] = c + sscCacheLock.Unlock() + return c, nil } diff --git a/motionplan/tpspace/ptgGroupFrame.go b/motionplan/tpspace/ptgGroupFrame.go index c53a9e72b9a..0fd3a0b313e 100644 --- a/motionplan/tpspace/ptgGroupFrame.go +++ b/motionplan/tpspace/ptgGroupFrame.go @@ -312,6 +312,37 @@ func (pf *ptgGroupFrame) validInputs(inputs []referenceframe.Input) error { return errAll } +// Hash returns a hash value for this PTG group frame. +func (pf *ptgGroupFrame) Hash() int { + hash := 0 + hash += hashString(pf.name) * 11 + hash += (5 * (int(pf.turnRadMillimeters*10) + 1000)) * 2 + hash += (6 * pf.trajCount) * 3 + hash += (7 * pf.correctionIdx) * 4 + + // Hash the limits + for i, limit := range pf.limits { + hash += ((i + 8) * (int(limit.Min*100) + 2000)) * (i + 5) + hash += ((i + 9) * (int(limit.Max*100) + 3000)) * (i + 6) + } + + // Hash geometries count (not the full geometry to avoid expensive computation) + hash += (10 * len(pf.geometries)) * 7 + + // Hash solvers count + hash += (11 * len(pf.solvers)) * 8 + + return hash +} + +func hashString(s string) int { + hash := 0 + for idx, c := range s { + hash += ((idx + 1) * 7) + ((int(c) + 12) * 12) + } + return hash +} + func initializePTGs(turnRadius float64, constructors []ptgFactory) []PTG { ptgs := []PTG{} for _, ptg := range constructors { diff --git a/motionplan/tpspace/ptgIKFrame.go b/motionplan/tpspace/ptgIKFrame.go index 35ec2e9131b..fe9c4f928d4 100644 --- a/motionplan/tpspace/ptgIKFrame.go +++ b/motionplan/tpspace/ptgIKFrame.go @@ -82,3 +82,18 @@ func (pf *ptgIKFrame) Interpolate(from, to []referenceframe.Input, by float64) ( // not be implemented until/unless it is guided by a specific need. return nil, errors.New("cannot interpolate ptg IK frames") } + +// Hash returns a hash value for this PTG IK frame. +func (pf *ptgIKFrame) Hash() int { + hash := 0 + // Hash the limits + for i, limit := range pf.limits { + hash += ((i + 5) * (int(limit.Min*100) + 1000)) * (i + 2) + hash += ((i + 6) * (int(limit.Max*100) + 2000)) * (i + 3) + } + // Hash the PTG interface - we use a simple marker since we can't access specific fields + if pf.PTG != nil { + hash += 12345 * 11 // Simple constant to indicate PTG presence + } + return hash +} diff --git a/pointcloud/basic_octree.go b/pointcloud/basic_octree.go index 3bee912b3a5..6d41c5f6489 100644 --- a/pointcloud/basic_octree.go +++ b/pointcloud/basic_octree.go @@ -351,6 +351,27 @@ func (octree *BasicOctree) Label() string { return octree.label } +// Hash returns a hash value for this octree. +func (octree *BasicOctree) Hash() int { + hash := 0 + hash += (5 * (int(octree.center.X*10) + 1000)) * 2 + hash += (6 * (int(octree.center.Y*10) + 2000)) * 3 + hash += (7 * (int(octree.center.Z*10) + 3000)) * 4 + hash += (8 * (int(octree.sideLength*10) + 4000)) * 5 + hash += (9 * octree.size) * 6 + hash += (10 * octree.confidenceThreshold) * 7 + hash += hashString(octree.label) * 11 + return hash +} + +func hashString(s string) int { + hash := 0 + for idx, c := range s { + hash += ((idx + 1) * 7) + ((int(c) + 12) * 12) + } + return hash +} + // String returns a human readable string that represents this octree. // octree's children will not be represented in the string. func (octree *BasicOctree) String() string { diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 3c5012010f4..9434e629e10 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -35,6 +35,14 @@ func (l *Limit) Range() float64 { return l.Max - l.Min } +// Hash returns a hash value for this limit. +func (l *Limit) Hash() int { + hash := 0 + hash += (5 * (int(l.Min*100) + 1000)) * 2 + hash += (6 * (int(l.Max*100) + 2000)) * 3 + return hash +} + const rangeLimit = 999 // GoodLimits gives min, max, range, but capped to -999,999. @@ -132,6 +140,8 @@ type Frame interface { // Name returns the name of the Frame Name() string + Hash() int + // Transform is the pose (rotation and translation) that goes FROM current frame TO parent's reference frame Transform([]Input) (spatial.Pose, error) @@ -158,6 +168,10 @@ type baseFrame struct { limits []Limit } +func (bf *baseFrame) hash() int { + return hashString(bf.name) + 10*len(bf.limits) +} + // Name returns the name of the Frame. func (bf *baseFrame) Name() string { return bf.name @@ -237,6 +251,11 @@ func (sf *tailGeometryStaticFrame) UnmarshalJSON(data []byte) error { return nil } +// Hash returns a hash value for this tail geometry static frame. +func (sf *tailGeometryStaticFrame) Hash() int { + return sf.staticFrame.Hash() + 7 // Add distinguishing factor for tail geometry placement +} + // namedFrame is used to change the name of a frame. type namedFrame struct { Frame @@ -256,6 +275,11 @@ func (nf *namedFrame) Geometries(inputs []Input) (*GeometriesInFrame, error) { return NewGeometriesInFrame(nf.name, gif.geometries), nil } +// Hash returns a hash value for this named frame. +func (nf *namedFrame) Hash() int { + return nf.Frame.Hash() + hashString(nf.name) +} + // NewNamedFrame will return a frame which has a new name but otherwise passes through all functions of the original frame. func NewNamedFrame(frame Frame, name string) Frame { return &namedFrame{Frame: frame, name: name} @@ -284,6 +308,17 @@ func NewStaticFrameWithGeometry(name string, pose spatial.Pose, geometry spatial return &staticFrame{&baseFrame{name, []Limit{}}, pose, geometry}, nil } +func (sf *staticFrame) Hash() int { + h := sf.hash() + if sf.transform != nil { + h += (123 * spatial.HashPose(sf.transform)) + } + if sf.geometry != nil { + h += +(111 * sf.geometry.Hash()) + } + return h +} + // Transform returns the pose associated with this static Frame. func (sf *staticFrame) Transform(input []Input) (spatial.Pose, error) { if len(input) != 0 { @@ -393,6 +428,15 @@ func NewTranslationalFrameWithGeometry(name string, axis r3.Vector, limit Limit, }, nil } +func (pf *translationalFrame) Hash() int { + h := pf.hash() + h += int(10000 * pf.transAxis.Norm()) + if pf.geometry != nil { + h += pf.geometry.Hash() + } + return h +} + // Transform returns a pose translated by the amount specified in the inputs. func (pf *translationalFrame) Transform(input []Input) (spatial.Pose, error) { err := pf.validInputs(input) @@ -488,6 +532,10 @@ func NewRotationalFrame(name string, axis spatial.R4AA, limit Limit) (Frame, err }, nil } +func (rf *rotationalFrame) Hash() int { + return rf.hash() + int(1000*rf.rotAxis.Norm()) +} + // Transform returns the Pose representing the frame's 6DoF motion in space. Requires a slice // of inputs that has length equal to the degrees of freedom of the Frame. func (rf *rotationalFrame) Transform(input []Input) (spatial.Pose, error) { @@ -574,6 +622,14 @@ func NewPoseFrame(name string, geometry []spatial.Geometry) (Frame, error) { }, nil } +func (pf *poseFrame) Hash() int { + h := pf.hash() + (111 * len(pf.geometries)) + for _, g := range pf.geometries { + h += g.Hash() + } + return h +} + // Transform on the poseFrame acts as the identity function. Whatever inputs are given are directly translated // in a 7DoF pose. We note that theta should be in radians. func (pf *poseFrame) Transform(inputs []Input) (spatial.Pose, error) { diff --git a/referenceframe/hash.go b/referenceframe/hash.go new file mode 100644 index 00000000000..d00e3367f24 --- /dev/null +++ b/referenceframe/hash.go @@ -0,0 +1,21 @@ +package referenceframe + +func (sfs *FrameSystem) Hash() int { + hash := len(sfs.frames) * 1000 + + for k, f := range sfs.frames { + // + is important + hash += hashString(k) + hash += f.Hash() + } + + return hash +} + +func hashString(s string) int { + hash := 0 + for idx, c := range s { + hash += ((idx + 1) * 7) + ((int(c) + 12) * 12) + } + return hash +} diff --git a/referenceframe/input.go b/referenceframe/input.go index 93e4c27f28b..a3892dc4e78 100644 --- a/referenceframe/input.go +++ b/referenceframe/input.go @@ -18,6 +18,11 @@ type Input struct { Value float64 } +// Hash returns a hash value for this input. +func (i *Input) Hash() int { + return (5 * (int(i.Value*100) + 1000)) * 2 +} + // JointPositionsFromInputs converts the given slice of Input to a JointPositions struct, // using the ProtobufFromInput function provided by the given Frame. func JointPositionsFromInputs(f Frame, inputs []Input) (*pb.JointPositions, error) { diff --git a/referenceframe/model.go b/referenceframe/model.go index 09016861eb8..c3b7ada35ee 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -122,6 +122,14 @@ func (m *SimpleModel) ModelConfig() *ModelConfigJSON { return m.modelConfig } +func (m *SimpleModel) Hash() int { + h := m.hash() + for _, f := range m.ordTransforms { + h += f.Hash() + } + return h +} + // Transform takes a model and a list of joint angles in radians and computes the dual quaternion representing the // cartesian position of the end effector. This is useful for when conversions between quaternions and OV are not needed. func (m *SimpleModel) Transform(inputs []Input) (spatialmath.Pose, error) { diff --git a/referenceframe/register_test.go b/referenceframe/register_test.go index 0e51c6ffc96..f4674270815 100644 --- a/referenceframe/register_test.go +++ b/referenceframe/register_test.go @@ -15,6 +15,10 @@ func (tF *trivialFrame) Name() string { return "" } +func (tF *trivialFrame) Hash() int { + return 123 +} + func (tF *trivialFrame) Transform([]Input) (spatial.Pose, error) { return nil, nil } diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 1485ab001ae..6b29ade1052 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -36,6 +36,10 @@ func (wf *wrapperFrame) Name() string { return wf.name } +func (wf *wrapperFrame) Hash() int { + return (17 * wf.localizationFrame.Hash()) + (31 * wf.executionFrame.Hash()) +} + // Transform returns the associated pose given a list of inputs. func (wf *wrapperFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { if len(inputs) != len(wf.DoF()) { diff --git a/spatialmath/axisAngle.go b/spatialmath/axisAngle.go index c429ccb383b..eddff06aaab 100644 --- a/spatialmath/axisAngle.go +++ b/spatialmath/axisAngle.go @@ -77,6 +77,16 @@ func (r4 *R4AA) ToQuat() quat.Number { return quat.Number{w, ax, ay, az} } +// Hash returns a hash value for this R4AA. +func (r4 *R4AA) Hash() int { + hash := 0 + hash += (5 * (int(r4.Theta*1000) + 1000)) * 2 + hash += (6 * (int(r4.RX*1000) + 2000)) * 3 + hash += (7 * (int(r4.RY*1000) + 3000)) * 4 + hash += (8 * (int(r4.RZ*1000) + 4000)) * 5 + return hash +} + // Normalize scales the x, y, and z components of a R4 axis angle to be on the unit sphere. func (r4 *R4AA) Normalize() { norm := math.Sqrt(r4.RX*r4.RX + r4.RY*r4.RY + r4.RZ*r4.RZ) diff --git a/spatialmath/box.go b/spatialmath/box.go index 46110fcfe1b..3630837d539 100644 --- a/spatialmath/box.go +++ b/spatialmath/box.go @@ -76,6 +76,10 @@ func NewBox(pose Pose, dims r3.Vector, label string) (Geometry, error) { }, nil } +func (b *box) Hash() int { + return HashPose(b.pose) + int((111*b.halfSize[0])+(222*b.halfSize[1])+(333*b.halfSize[2])) +} + // String returns a human readable string that represents the box. func (b *box) String() string { return fmt.Sprintf("Type: Box | Position: X:%.1f, Y:%.1f, Z:%.1f | Dims: X:%.0f, Y:%.0f, Z:%.0f", diff --git a/spatialmath/capsule.go b/spatialmath/capsule.go index 3f02a199db9..bd12aaa33a5 100644 --- a/spatialmath/capsule.go +++ b/spatialmath/capsule.go @@ -229,6 +229,15 @@ func (c *capsule) ToPoints(resolution float64) []r3.Vector { return transformPointsToPose(vecList, c.Pose()) } +// Hash returns a hash value for this capsule. +func (c *capsule) Hash() int { + hash := HashPose(c.pose) + hash += (8 * (int(c.radius*100) + 3000)) * 9 + hash += (9 * (int(c.length*100) + 4000)) * 10 + hash += hashString(c.label) * 11 + return hash +} + // rotationMatrix returns the cached matrix if it exists, and generates it if not. func (c *capsule) rotationMatrix() *RotationMatrix { c.once.Do(func() { c.rotMatrix = c.pose.Orientation().RotationMatrix() }) diff --git a/spatialmath/dualquaternion.go b/spatialmath/dualquaternion.go index c7924a98913..16b46ce0e06 100644 --- a/spatialmath/dualquaternion.go +++ b/spatialmath/dualquaternion.go @@ -232,3 +232,8 @@ func OffsetBy(a, b *commonpb.Pose) *commonpb.Pose { return q3.ToProtobuf() } + +// Hash returns a hash value for this dual quaternion. +func (q *dualQuaternion) Hash() int { + return HashPose(q) +} diff --git a/spatialmath/eulerangles.go b/spatialmath/eulerangles.go index b5af6a58403..4fb58ac67fd 100644 --- a/spatialmath/eulerangles.go +++ b/spatialmath/eulerangles.go @@ -61,3 +61,12 @@ func (ea *EulerAngles) AxisAngles() *R4AA { func (ea *EulerAngles) RotationMatrix() *RotationMatrix { return QuatToRotationMatrix(ea.Quaternion()) } + +// Hash returns a hash value for these Euler angles. +func (ea *EulerAngles) Hash() int { + hash := 0 + hash += (5 * (int(ea.Roll*1000) + 1000)) * 2 + hash += (6 * (int(ea.Pitch*1000) + 2000)) * 3 + hash += (7 * (int(ea.Yaw*1000) + 3000)) * 4 + return hash +} diff --git a/spatialmath/geometry.go b/spatialmath/geometry.go index aebe0fad41f..15893817e96 100644 --- a/spatialmath/geometry.go +++ b/spatialmath/geometry.go @@ -47,6 +47,8 @@ type Geometry interface { // ToProtobuf converts a Geometry to its protobuf representation. ToProtobuf() *commonpb.Geometry + Hash() int + json.Marshaler } diff --git a/spatialmath/mesh.go b/spatialmath/mesh.go index d12a7475597..4995bcb625d 100644 --- a/spatialmath/mesh.go +++ b/spatialmath/mesh.go @@ -642,3 +642,18 @@ func (m *Mesh) TrianglesToPLYBytes(convertToWorldFrame bool) []byte { return buf.Bytes() } + +// Hash returns a hash value for this mesh. +func (m *Mesh) Hash() int { + hash := HashPose(m.pose) + hash += hashString(m.label) * 11 + hash += len(m.triangles) * 12 + // Include a sample of triangle hashes for efficiency + for i, tri := range m.triangles { + if i >= 10 { // Only hash first 10 triangles for performance + break + } + hash += tri.Hash() * (13 + i) + } + return hash +} diff --git a/spatialmath/orientationVector.go b/spatialmath/orientationVector.go index 917e7a902c1..c3acdf7d9c7 100644 --- a/spatialmath/orientationVector.go +++ b/spatialmath/orientationVector.go @@ -207,3 +207,23 @@ func (ovd *OrientationVectorDegrees) AxisAngles() *R4AA { func (ovd *OrientationVectorDegrees) RotationMatrix() *RotationMatrix { return QuatToRotationMatrix(ovd.Quaternion()) } + +// Hash returns a hash value for this orientation vector. +func (ov *OrientationVector) Hash() int { + hash := 0 + hash += (5 * (int(ov.Theta*1000) + 1000)) * 2 + hash += (6 * (int(ov.OX*1000) + 2000)) * 3 + hash += (7 * (int(ov.OY*1000) + 3000)) * 4 + hash += (8 * (int(ov.OZ*1000) + 4000)) * 5 + return hash +} + +// Hash returns a hash value for this orientation vector. +func (ovd *OrientationVectorDegrees) Hash() int { + hash := 0 + hash += (5 * (int(ovd.Theta*100) + 1000)) * 2 + hash += (6 * (int(ovd.OX*1000) + 2000)) * 3 + hash += (7 * (int(ovd.OY*1000) + 3000)) * 4 + hash += (8 * (int(ovd.OZ*1000) + 4000)) * 5 + return hash +} diff --git a/spatialmath/point.go b/spatialmath/point.go index 4753446947a..808a9cfac69 100644 --- a/spatialmath/point.go +++ b/spatialmath/point.go @@ -134,6 +134,25 @@ func pointVsBoxDistance(pt r3.Vector, b *box) float64 { return -b.pointPenetrationDepth(pt) } +// Hash returns a hash value for this point. +func (pt *point) Hash() int { + hash := 0 + pos := pt.position + hash += (5 * (int(pos.X*10) + 1000)) * 2 + hash += (6 * (int(pos.Y*10) + 10221)) * 3 + hash += (7 * (int(pos.Z*10) + 2124)) * 4 + hash += hashString(pt.label) * 11 + return hash +} + +func hashString(s string) int { + hash := 0 + for idx, c := range s { + hash += ((idx + 1) * 7) + ((int(c) + 12) * 12) + } + return hash +} + // ToPointCloud converts a point geometry into a []r3.Vector. func (pt *point) ToPoints(resolution float64) []r3.Vector { return []r3.Vector{pt.position} diff --git a/spatialmath/pose.go b/spatialmath/pose.go index 7d6fdc95ffb..18de0e34cba 100644 --- a/spatialmath/pose.go +++ b/spatialmath/pose.go @@ -201,6 +201,11 @@ func (d *distancePose) Orientation() Orientation { return (*Quaternion)(&d.orientation) } +// Hash returns a hash value for this distance pose. +func (d *distancePose) Hash() int { + return HashPose(d) +} + // ResetPoseDQTranslation takes a Pose that must be a dualQuaternion and reset's it's translation. func ResetPoseDQTranslation(p Pose, v r3.Vector) { q, ok := p.(*dualQuaternion) @@ -232,3 +237,21 @@ func ProjectOrientationTo2dRotation(pose Pose) (Pose, error) { theta := -math.Atan2(newAdjPt.Y, -newAdjPt.X) return NewPose(pose.Point(), &OrientationVector{OZ: 1, Theta: theta}), nil } + +func HashPose(p Pose) int { + hash := 0 + + pp := p.Point() + + hash += (5 * (int(pp.X*10) + 100)) * 2 + hash += (6 * (int(pp.Y*10) + 10221)) * 3 + hash += (7 * (int(pp.Z*10) + 2124)) * 4 + + o := p.Orientation().OrientationVectorDegrees() + hash += (8 * (int(o.OX*100) + 2313)) * 5 + hash += (9 * (int(o.OY*100) + 3133)) * 6 + hash += (10 * (int(o.OZ*100) + 2931)) * 7 + hash += (11 * (int(o.Theta*10) + 6315)) * 8 + + return hash +} diff --git a/spatialmath/quaternion.go b/spatialmath/quaternion.go index 4038482b619..1f89f29c46d 100644 --- a/spatialmath/quaternion.go +++ b/spatialmath/quaternion.go @@ -310,3 +310,13 @@ func quaternionJSONFromQuaternion(q *Quaternion) quaternionJSON { Z: q.Kmag, } } + +// Hash returns a hash value for this quaternion. +func (q *Quaternion) Hash() int { + hash := 0 + hash += (5 * (int(q.Real*1000) + 1000)) * 2 + hash += (6 * (int(q.Imag*1000) + 2000)) * 3 + hash += (7 * (int(q.Jmag*1000) + 3000)) * 4 + hash += (8 * (int(q.Kmag*1000) + 4000)) * 5 + return hash +} diff --git a/spatialmath/rotationMatrix.go b/spatialmath/rotationMatrix.go index 53453d9d6f6..b8fcf0ddafe 100644 --- a/spatialmath/rotationMatrix.go +++ b/spatialmath/rotationMatrix.go @@ -151,3 +151,12 @@ func (rm *RotationMatrix) RightMatMul(rmm RotationMatrix) *RotationMatrix { } return &RotationMatrix{mat: mat} } + +// Hash returns a hash value for this rotation matrix. +func (rm *RotationMatrix) Hash() int { + hash := 0 + for i, val := range rm.mat { + hash += ((i + 5) * (int(val*1000) + 1000)) * (i + 2) + } + return hash +} diff --git a/spatialmath/sphere.go b/spatialmath/sphere.go index 91008c365dd..7212de41db0 100644 --- a/spatialmath/sphere.go +++ b/spatialmath/sphere.go @@ -39,6 +39,10 @@ func (s sphere) MarshalJSON() ([]byte, error) { return json.Marshal(config) } +func (s *sphere) Hash() int { + return HashPose(s.pose) + int((117 * s.radius)) +} + // String returns a human readable string that represents the sphere. func (s *sphere) String() string { return fmt.Sprintf("Type: Sphere | Position: X:%.1f, Y:%.1f, Z:%.1f | Radius: %.0f", diff --git a/spatialmath/triangle.go b/spatialmath/triangle.go index 002a05a329e..6bf47319327 100644 --- a/spatialmath/triangle.go +++ b/spatialmath/triangle.go @@ -109,3 +109,18 @@ func ClosestPointTrianglePoint(t *Triangle, point r3.Vector) r3.Vector { } return closestPt } + +// Hash returns a hash value for this triangle. +func (t *Triangle) Hash() int { + hash := 0 + hash += (5 * (int(t.p0.X*10) + 1000)) * 2 + hash += (6 * (int(t.p0.Y*10) + 10221)) * 3 + hash += (7 * (int(t.p0.Z*10) + 2124)) * 4 + hash += (8 * (int(t.p1.X*10) + 5000)) * 5 + hash += (9 * (int(t.p1.Y*10) + 6000)) * 6 + hash += (10 * (int(t.p1.Z*10) + 7000)) * 7 + hash += (11 * (int(t.p2.X*10) + 8000)) * 8 + hash += (12 * (int(t.p2.Y*10) + 9000)) * 9 + hash += (13 * (int(t.p2.Z*10) + 10000)) * 10 + return hash +} From d55135e94f16627181c88111b10cbf35e5e1313a Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 22 Oct 2025 09:19:33 -0400 Subject: [PATCH 13/20] lint --- motionplan/armplanning/smart_seed.go | 4 ++-- referenceframe/hash.go | 1 + referenceframe/model.go | 1 + spatialmath/pose.go | 1 + 4 files changed, 5 insertions(+), 2 deletions(-) diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index 9fa16377511..a4dbd18b911 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -260,7 +260,7 @@ func (ssc *smartSeedCache) buildRawCache(values []float64, joint int) error { } func (ssc *smartSeedCache) buildCache(logger logging.Logger) error { - logger.Debugf("buildCache %v", ssc.lfs.dof) + logger.Debugf("buildCache %d %v", len(ssc.fs.FrameNames()), ssc.lfs.dof) values := make([]float64, len(ssc.lfs.dof)) err := ssc.buildRawCache(values, 0) @@ -311,7 +311,7 @@ func (ssc *smartSeedCache) buildInverseCache(frame string) { } var ( - sscCache map[int]*smartSeedCache = map[int]*smartSeedCache{} + sscCache = map[int]*smartSeedCache{} sscCacheLock sync.Mutex ) diff --git a/referenceframe/hash.go b/referenceframe/hash.go index d00e3367f24..9be7360d000 100644 --- a/referenceframe/hash.go +++ b/referenceframe/hash.go @@ -1,5 +1,6 @@ package referenceframe +// Hash returns a hash value for this frame system. func (sfs *FrameSystem) Hash() int { hash := len(sfs.frames) * 1000 diff --git a/referenceframe/model.go b/referenceframe/model.go index c3b7ada35ee..dd49ce96431 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -122,6 +122,7 @@ func (m *SimpleModel) ModelConfig() *ModelConfigJSON { return m.modelConfig } +// Hash returns a hash value for this simple model. func (m *SimpleModel) Hash() int { h := m.hash() for _, f := range m.ordTransforms { diff --git a/spatialmath/pose.go b/spatialmath/pose.go index 18de0e34cba..4ff731fb242 100644 --- a/spatialmath/pose.go +++ b/spatialmath/pose.go @@ -238,6 +238,7 @@ func ProjectOrientationTo2dRotation(pose Pose) (Pose, error) { return NewPose(pose.Point(), &OrientationVector{OZ: 1, Theta: theta}), nil } +// HashPose returns a hash value for the given pose. func HashPose(p Pose) int { hash := 0 From 59f1e3f7999f6899c9018abbfe012a621ce0283e Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 22 Oct 2025 13:35:50 -0400 Subject: [PATCH 14/20] closer --- motionplan/armplanning/smart_seed.go | 294 +++++++++++++--------- motionplan/armplanning/smart_seed_test.go | 71 +++++- 2 files changed, 233 insertions(+), 132 deletions(-) diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index a4dbd18b911..40f29246ac9 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -15,8 +15,8 @@ import ( ) type smartSeedCacheEntry struct { - inputs referenceframe.FrameSystemInputs - poses referenceframe.FrameSystemPoses + inputs []referenceframe.Input + pose spatialmath.Pose // this is in the frame's frame, NOT world } type goalCacheBox struct { @@ -25,46 +25,72 @@ type goalCacheBox struct { entries []smartSeedCacheEntry } -type goalCache struct { +type cacheForFrame struct { + entries []smartSeedCacheEntry + minCartesian, maxCartesian r3.Vector - boxes map[string]*goalCacheBox // hash to list + + boxes map[string]*goalCacheBox // hash to list } -func (gc *goalCache) boxKeyCompute(value, min, max float64) int { +func (cff *cacheForFrame) boxKeyCompute(value, min, max float64) int { x := (value - min) / (max - min) - return int(x * 100) } -func (gc *goalCache) boxKey(p r3.Vector) string { - x := gc.boxKeyCompute(p.X, gc.minCartesian.X, gc.maxCartesian.X) - y := gc.boxKeyCompute(p.Y, gc.minCartesian.Y, gc.maxCartesian.Y) - z := gc.boxKeyCompute(p.Z, gc.minCartesian.Z, gc.maxCartesian.Z) +func (cff *cacheForFrame) boxKey(p r3.Vector) string { + x := cff.boxKeyCompute(p.X, cff.minCartesian.X, cff.maxCartesian.X) + y := cff.boxKeyCompute(p.Y, cff.minCartesian.Y, cff.maxCartesian.Y) + z := cff.boxKeyCompute(p.Z, cff.minCartesian.Z, cff.maxCartesian.Z) return fmt.Sprintf("%0.3d%0.3d%0.3d", x, y, z) } -type smartSeedCache struct { - fs *referenceframe.FrameSystem - lfs *linearizedFrameSystem +func (cff *cacheForFrame) add(e smartSeedCacheEntry) { + cff.entries = append(cff.entries, e) +} - rawCache []smartSeedCacheEntry +func (cff *cacheForFrame) buildInverseCache() { + cff.boxes = map[string]*goalCacheBox{} - geoCache map[string]*goalCache + for _, e := range cff.entries { + cff.minCartesian.X = min(cff.minCartesian.X, e.pose.Point().X) + cff.minCartesian.Y = min(cff.minCartesian.X, e.pose.Point().Y) + cff.minCartesian.Z = min(cff.minCartesian.X, e.pose.Point().X) + + cff.maxCartesian.X = max(cff.maxCartesian.X, e.pose.Point().X) + cff.maxCartesian.Y = max(cff.maxCartesian.X, e.pose.Point().Y) + cff.maxCartesian.Z = max(cff.maxCartesian.X, e.pose.Point().X) + } + + for _, e := range cff.entries { + key := cff.boxKey(e.pose.Point()) + box, ok := cff.boxes[key] + if !ok { + box = &goalCacheBox{boxKey: key} + cff.boxes[key] = box + } + box.entries = append(box.entries, e) + } + + for _, v := range cff.boxes { + for _, e := range v.entries { + p := e.pose.Point() + v.center = v.center.Add(p) + } + + v.center = v.center.Mul(1.0 / float64(len(v.entries))) + } } -func (ssc *smartSeedCache) findBoxes(goalFrame string, goalPose spatialmath.Pose) []*goalCacheBox { +func (cff *cacheForFrame) findBoxes(goalPose spatialmath.Pose) []*goalCacheBox { type e struct { b *goalCacheBox d float64 } - if ssc.geoCache[goalFrame] == nil { - ssc.buildInverseCache(goalFrame) - } - best := []e{} - for _, b := range ssc.geoCache[goalFrame].boxes { + for _, b := range cff.boxes { d := goalPose.Point().Distance(b.center) best = append(best, e{b, d}) } @@ -82,6 +108,42 @@ func (ssc *smartSeedCache) findBoxes(goalFrame string, goalPose spatialmath.Pose return boxes } +type smartSeedCache struct { + fs *referenceframe.FrameSystem + lfs *linearizedFrameSystem + + rawCache map[string]*cacheForFrame +} + +func (ssc *smartSeedCache) findMovingInfo(inputs referenceframe.FrameSystemInputs, + goalFrame string, goalPIF *referenceframe.PoseInFrame, +) (string, spatialmath.Pose, error) { + var err error + frame := ssc.fs.Frame(goalFrame) + if frame == nil { + return "", nil, fmt.Errorf("no frame for %v", goalFrame) + } + for { + if len(frame.DoF()) > 0 { + break + } + if frame == ssc.fs.World() { + return "", nil, fmt.Errorf("hit world, and no moving parts when looking to move %s", goalFrame) + } + frame, err = ssc.fs.Parent(frame) + if err != nil { + return "", nil, err + } + } + + newPif, err := ssc.fs.Transform(inputs, goalPIF, frame.Name()) + if err != nil { + return "", nil, fmt.Errorf("cannot transform %s to %s: %w", goalPIF.Parent(), frame.Name(), err) + } + + return frame.Name(), newPif.(*referenceframe.PoseInFrame).Pose(), nil +} + func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, start referenceframe.FrameSystemInputs, logger logging.Logger, @@ -104,42 +166,74 @@ func (ssc *smartSeedCache) findSeeds(goal referenceframe.FrameSystemPoses, return nil, fmt.Errorf("smartSeedCache findSeed only works with 1 goal for now") } - for _, p := range goal { - if p.Parent() != referenceframe.World { - return nil, fmt.Errorf("goal has to be in world, not %s", p.Parent()) - } - } - goalFrame := "" - var goalPose spatialmath.Pose + var goalPIF *referenceframe.PoseInFrame for k, v := range goal { goalFrame = k - goalPose = v.Pose() + goalPIF = v + } + + movingFrame, movingPose, err := ssc.findMovingInfo(start, goalFrame, goalPIF) + if err != nil { + return nil, err + } + + seeds, err := ssc.findSeedsForFrame(movingFrame, start[movingFrame], movingPose, logger) + if err != nil { + return nil, err + } + + fullSeeds := []referenceframe.FrameSystemInputs{} + for _, s := range seeds { + i := referenceframe.FrameSystemInputs{} + for k, v := range start { + i[k] = v + } + i[goalFrame] = s + fullSeeds = append(fullSeeds, i) + } + + return fullSeeds, nil +} + +func (ssc *smartSeedCache) findSeedsForFrame( + frameName string, + start []referenceframe.Input, + goalPose spatialmath.Pose, + logger logging.Logger, +) ([][]referenceframe.Input, error) { + frame := ssc.fs.Frame(frameName) + if frame == nil { + return nil, fmt.Errorf("no frame %s", frameName) } + logger.Debugf("findSeedsForFrame: %s goalPose: %v", frameName, goalPose) + type entry struct { e *smartSeedCacheEntry distance float64 cost float64 } - best := []entry{} - - startPoses, err := start.ComputePoses(ssc.fs) + startPose, err := frame.Transform(start) if err != nil { return nil, err } - startDistance := max(1, ssc.distance(startPoses, goal)) - logger.Debugf("startDistance: %v", startDistance) + startDistance := max(1, motionplan.WeightedSquaredNormDistance(startPose, goalPose)) + + best := []entry{} + bestDistance := startDistance * 2 - boxes := ssc.findBoxes(goalFrame, goalPose) + boxes := ssc.rawCache[frameName].findBoxes(goalPose) + + logger.Debugf("startDistance: %v num boxes: %d", startDistance, len(boxes)) for _, b := range boxes { for _, c := range b.entries { - distance := ssc.distance(goal, c.poses) + distance := motionplan.WeightedSquaredNormDistance(goalPose, c.pose) if distance > (bestDistance * 2) { continue } @@ -148,10 +242,7 @@ func (ssc *smartSeedCache) findSeeds(goal referenceframe.FrameSystemPoses, bestDistance = distance } - cost := 0.0 - for k, j := range start { - cost += referenceframe.InputsL2Distance(j, c.inputs[k]) - } + cost := referenceframe.InputsL2Distance(start, c.inputs) best = append(best, entry{&c, distance, cost}) } @@ -159,7 +250,7 @@ func (ssc *smartSeedCache) findSeeds(goal referenceframe.FrameSystemPoses, if len(best) == 0 { logger.Debugf("no best, returning start") - return []referenceframe.FrameSystemInputs{start}, nil + return [][]referenceframe.Input{start}, nil } sort.Slice(best, func(i, j int) bool { @@ -174,81 +265,54 @@ func (ssc *smartSeedCache) findSeeds(goal referenceframe.FrameSystemPoses, cutIdx++ } + logger.Debugf("\t len(best): %d cutIdx: %d", len(best), cutIdx) + best = best[0:cutIdx] sort.Slice(best, func(i, j int) bool { return best[i].cost < best[j].cost }) - ret := []referenceframe.FrameSystemInputs{} + ret := [][]referenceframe.Input{} for i := 0; i < len(best) && i < 5; i++ { e := best[i] ret = append(ret, e.e.inputs) - // logger.Debugf("%v dist: %02.f cost: %0.2f", e.e.inputs, e.distance, e.cost) + // logger.Debugf("dist: %02.f cost: %0.2f %v", e.distance, e.cost, e.e.inputs) } return ret, nil } -func (ssc *smartSeedCache) distance(a, b referenceframe.FrameSystemPoses) float64 { - dist := 0.0 - - for k, p := range a { - if p.Parent() != referenceframe.World { - panic(fmt.Errorf("eliot fucked up %s", p.Parent())) - } - - pp, ok := b[k] - if !ok { - continue - } - - if pp == nil || pp.Parent() != referenceframe.World { - panic(fmt.Errorf("eliot fucked up %s", pp)) - } - - dist += motionplan.WeightedSquaredNormDistance(p.Pose(), pp.Pose()) - } - - return dist -} - -func (ssc *smartSeedCache) addToCache(values []float64) error { - inputs, err := ssc.lfs.sliceToMap(values) - if err != nil { - return err - } - poses, err := inputs.ComputePoses(ssc.fs) +func (ssc *smartSeedCache) addToCache(frameName string, frame referenceframe.Frame, values []float64) error { + inputs := referenceframe.FloatsToInputs(values) + p, err := frame.Transform(inputs) if err != nil { return err } - for _, p := range poses { - if p.Parent() != referenceframe.World { - return fmt.Errorf("why not in world, but %s", p.Parent()) - } - } + ssc.rawCache[frameName].add(smartSeedCacheEntry{inputs, p}) - ssc.rawCache = append(ssc.rawCache, smartSeedCacheEntry{inputs, poses}) return nil } -func (ssc *smartSeedCache) buildRawCache(values []float64, joint int) error { - if joint > len(ssc.lfs.dof) { - panic(fmt.Errorf("joint: %d > len(ssc.lfs.dof): %d", joint, len(ssc.lfs.dof))) +func (ssc *smartSeedCache) buildRawCache(frameName string, f referenceframe.Frame, values []float64, joint int) error { + limits := f.DoF() + + if joint > len(limits) { + panic(fmt.Errorf("joint: %d > len(limits): %d", joint, len(limits))) } - if joint == len(ssc.lfs.dof) { - return ssc.addToCache(values) + if joint == len(limits) { + return ssc.addToCache(frameName, f, values) } - min, max, r := ssc.lfs.dof[joint].GoodLimits() + min, max, r := limits[joint].GoodLimits() values[joint] = min jog := r / 10 for values[joint] <= max { - err := ssc.buildRawCache(values, joint+1) + err := ssc.buildRawCache(frameName, f, values, joint+1) if err != nil { return err } @@ -259,55 +323,43 @@ func (ssc *smartSeedCache) buildRawCache(values []float64, joint int) error { return nil } -func (ssc *smartSeedCache) buildCache(logger logging.Logger) error { - logger.Debugf("buildCache %d %v", len(ssc.fs.FrameNames()), ssc.lfs.dof) +func (ssc *smartSeedCache) buildCacheForFrame(frameName string) error { + f := ssc.fs.Frame(frameName) + if f == nil { + return fmt.Errorf("no frame: %s", f) + } - values := make([]float64, len(ssc.lfs.dof)) - err := ssc.buildRawCache(values, 0) - if err != nil { - return fmt.Errorf("cannot buildCache: %w", err) + if len(f.DoF()) == 0 { + return nil } - ssc.geoCache = map[string]*goalCache{} + ssc.rawCache[frameName] = &cacheForFrame{} - return nil -} + values := make([]float64, len(f.DoF())) -func (ssc *smartSeedCache) buildInverseCache(frame string) { - gc := &goalCache{ - boxes: map[string]*goalCacheBox{}, + err := ssc.buildRawCache(frameName, f, values, 0) + if err != nil { + return fmt.Errorf("cannot buildCache for: %s: %w", frameName, err) } - for _, e := range ssc.rawCache { - gc.minCartesian.X = min(gc.minCartesian.X, e.poses[frame].Pose().Point().X) - gc.minCartesian.Y = min(gc.minCartesian.X, e.poses[frame].Pose().Point().Y) - gc.minCartesian.Z = min(gc.minCartesian.X, e.poses[frame].Pose().Point().X) + ssc.rawCache[frameName].buildInverseCache() - gc.maxCartesian.X = max(gc.maxCartesian.X, e.poses[frame].Pose().Point().X) - gc.maxCartesian.Y = max(gc.maxCartesian.X, e.poses[frame].Pose().Point().Y) - gc.maxCartesian.Z = max(gc.maxCartesian.X, e.poses[frame].Pose().Point().X) - } + return nil +} - for _, e := range ssc.rawCache { - key := gc.boxKey(e.poses[frame].Pose().Point()) - box, ok := gc.boxes[key] - if !ok { - box = &goalCacheBox{boxKey: key} - gc.boxes[key] = box - } - box.entries = append(box.entries, e) - } +func (ssc *smartSeedCache) buildCache(logger logging.Logger) error { + logger.Debugf("buildCache %d %v", len(ssc.fs.FrameNames()), ssc.lfs.dof) - for _, v := range gc.boxes { - for _, e := range v.entries { - p := e.poses[frame].Pose().Point() - v.center = v.center.Add(p) - } + ssc.rawCache = map[string]*cacheForFrame{} - v.center = v.center.Mul(1.0 / float64(len(v.entries))) + for _, frameName := range ssc.fs.FrameNames() { + err := ssc.buildCacheForFrame(frameName) + if err != nil { + return fmt.Errorf("cannot build cache for frame: %s", frameName) + } } - ssc.geoCache[frame] = gc + return nil } var ( diff --git a/motionplan/armplanning/smart_seed_test.go b/motionplan/armplanning/smart_seed_test.go index 8a6f320c5d1..ffa2926c042 100644 --- a/motionplan/armplanning/smart_seed_test.go +++ b/motionplan/armplanning/smart_seed_test.go @@ -36,19 +36,68 @@ func TestSmartSeedCache1(t *testing.T) { {-3.681258507284093}, }} - startTime := time.Now() - seed, err := c.findSeed( - referenceframe.FrameSystemPoses{"ur5e": referenceframe.NewPoseInFrame("world", - spatialmath.NewPose( - r3.Vector{X: -337.976430, Y: -464.051182, Z: 554.695381}, - &spatialmath.OrientationVectorDegrees{OX: 0.499987, OY: -0.866033, OZ: -0.000000, Theta: 0.000000}, - ))}, - start, logger) + goal := spatialmath.NewPose( + r3.Vector{X: -337.976430, Y: -464.051182, Z: 554.695381}, + &spatialmath.OrientationVectorDegrees{OX: 0.499987, OY: -0.866033, OZ: -0.000000, Theta: 0.000000}, + ) + + t.Run("partial", func(t *testing.T) { + startTime := time.Now() + seeds, err := c.findSeedsForFrame( + "ur5e", + start["ur5e"], + goal, + logger) + test.That(t, err, test.ShouldBeNil) + logger.Infof("time to run findSeedsForFrame: %v", time.Since(startTime)) + + cost := referenceframe.InputsL2Distance(start["ur5e"], seeds[0]) + test.That(t, cost, test.ShouldBeLessThan, 1.25) + }) + + t.Run("real", func(t *testing.T) { + startTime := time.Now() + seed, err := c.findSeed( + referenceframe.FrameSystemPoses{"ur5e": referenceframe.NewPoseInFrame("world", goal)}, + start, + logger) + test.That(t, err, test.ShouldBeNil) + logger.Infof("time to run findSeed: %v", time.Since(startTime)) + + cost := referenceframe.InputsL2Distance(start["ur5e"], seed["ur5e"]) + test.That(t, cost, test.ShouldBeLessThan, 1.25) + }) +} + +func TestSmartSeedCacheFrames(t *testing.T) { + logger := logging.NewTestLogger(t) + + armName := "arm" + armKinematics, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/kinematics/ur5e.json"), armName) + test.That(t, err, test.ShouldBeNil) + + gripperFrame, err := referenceframe.NewStaticFrame("gripper", spatialmath.NewPoseFromPoint(r3.Vector{10, 10, 10})) test.That(t, err, test.ShouldBeNil) - logger.Infof("time to run findSeed: %v", time.Since(startTime)) - cost := referenceframe.InputsL2Distance(start["ur5e"], seed["ur5e"]) - test.That(t, cost, test.ShouldBeLessThan, 1.25) + t.Run("gripper", func(t *testing.T) { + fs := referenceframe.NewEmptyFrameSystem("pirouette") + + err = fs.AddFrame(armKinematics, fs.World()) + test.That(t, err, test.ShouldBeNil) + err = fs.AddFrame(gripperFrame, fs.Frame("arm")) + test.That(t, err, test.ShouldBeNil) + + c, err := smartSeed(fs, logger) + test.That(t, err, test.ShouldBeNil) + + f, p, err := c.findMovingInfo(referenceframe.FrameSystemInputs{ + "arm": []referenceframe.Input{{0}, {0}, {0}, {0}, {0}, {0}}, + }, + "gripper", referenceframe.NewPoseInFrame("world", spatialmath.NewPose(r3.Vector{}, &spatialmath.OrientationVectorDegrees{}))) + test.That(t, err, test.ShouldBeNil) + test.That(t, f, test.ShouldEqual, "arm") + test.That(t, p.Point(), test.ShouldResemble, r3.Vector{100, 100, 100}) + }) } func TestSmartSeedCachePirouette(t *testing.T) { From fd2a838379f555b00d67c7beaff643ccb63d267e Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 22 Oct 2025 14:25:54 -0400 Subject: [PATCH 15/20] working --- motionplan/armplanning/smart_seed.go | 33 ++++++++++++++++++++--- motionplan/armplanning/smart_seed_test.go | 15 +++++++---- referenceframe/frame_system.go | 7 ++--- 3 files changed, 44 insertions(+), 11 deletions(-) diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index 40f29246ac9..b58edb91d25 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -136,12 +136,39 @@ func (ssc *smartSeedCache) findMovingInfo(inputs referenceframe.FrameSystemInput } } - newPif, err := ssc.fs.Transform(inputs, goalPIF, frame.Name()) + // there are 3 frames at play here + // 1) the frame the goal is specified in + // 2) the frame of the thing we want to move + // 3) the frame of the actuating component + + f2w1, err := ssc.fs.GetFrameToWorldTransform(inputs, ssc.fs.Frame(goalPIF.Parent())) + if err != nil { + return "", nil, err + } + f2w2, err := ssc.fs.GetFrameToWorldTransform(inputs, ssc.fs.Frame(goalFrame)) + if err != nil { + return "", nil, err + } + f2w3, err := ssc.fs.GetFrameToWorldTransform(inputs, ssc.fs.Frame(frame.Name())) if err != nil { - return "", nil, fmt.Errorf("cannot transform %s to %s: %w", goalPIF.Parent(), frame.Name(), err) + return "", nil, err } - return frame.Name(), newPif.(*referenceframe.PoseInFrame).Pose(), nil + goalInWorld := spatialmath.Compose(goalPIF.Pose(), f2w1) + delta := spatialmath.Compose(f2w2, spatialmath.PoseInverse(f2w3)) + + newPose := spatialmath.Compose(goalInWorld, delta) + + /* + fmt.Printf("f2w1: %v\n", f2w1) + fmt.Printf("f2w2: %v\n", f2w2) + fmt.Printf("f2w3: %v\n", f2w3) + fmt.Printf("goalInWorld: %v\n", goalInWorld) + fmt.Printf("delta: %v\n", delta) + fmt.Printf("eliot: %v -> %v\n", goalPIF, newPose) + */ + + return frame.Name(), newPose, nil } func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, diff --git a/motionplan/armplanning/smart_seed_test.go b/motionplan/armplanning/smart_seed_test.go index ffa2926c042..4bfdba4214f 100644 --- a/motionplan/armplanning/smart_seed_test.go +++ b/motionplan/armplanning/smart_seed_test.go @@ -90,13 +90,18 @@ func TestSmartSeedCacheFrames(t *testing.T) { c, err := smartSeed(fs, logger) test.That(t, err, test.ShouldBeNil) - f, p, err := c.findMovingInfo(referenceframe.FrameSystemInputs{ - "arm": []referenceframe.Input{{0}, {0}, {0}, {0}, {0}, {0}}, - }, - "gripper", referenceframe.NewPoseInFrame("world", spatialmath.NewPose(r3.Vector{}, &spatialmath.OrientationVectorDegrees{}))) + f, p, err := c.findMovingInfo( + referenceframe.FrameSystemInputs{ + "arm": []referenceframe.Input{{0}, {0}, {0}, {0}, {0}, {0}}, + }, + "gripper", + referenceframe.NewPoseInFrame("world", spatialmath.NewPose(r3.Vector{}, &spatialmath.OrientationVectorDegrees{})), + ) test.That(t, err, test.ShouldBeNil) test.That(t, f, test.ShouldEqual, "arm") - test.That(t, p.Point(), test.ShouldResemble, r3.Vector{100, 100, 100}) + test.That(t, p.Point().X, test.ShouldAlmostEqual, 10) + test.That(t, p.Point().Y, test.ShouldAlmostEqual, -10) + test.That(t, p.Point().Z, test.ShouldAlmostEqual, 10) }) } diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index dfbbf9d4c32..20a25435893 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -350,7 +350,8 @@ func (sfs *FrameSystem) DivideFrameSystem(newRoot Frame) (*FrameSystem, error) { return newFS, nil } -func (sfs *FrameSystem) getFrameToWorldTransform(inputMap FrameSystemInputs, src Frame) (spatial.Pose, error) { +// GetFrameToWorldTransform computes the position of src in the world frame based on inputMap. +func (sfs *FrameSystem) GetFrameToWorldTransform(inputMap FrameSystemInputs, src Frame) (spatial.Pose, error) { if !sfs.frameExists(src.Name()) { return nil, NewFrameMissingError(src.Name()) } @@ -402,11 +403,11 @@ func (sfs *FrameSystem) ReplaceFrame(replacementFrame Frame) error { // Returns the relative pose between the parent and the destination frame. func (sfs *FrameSystem) transformFromParent(inputMap FrameSystemInputs, src, dst Frame) (*PoseInFrame, error) { - dstToWorld, err := sfs.getFrameToWorldTransform(inputMap, dst) + dstToWorld, err := sfs.GetFrameToWorldTransform(inputMap, dst) if err != nil { return nil, err } - srcToWorld, err := sfs.getFrameToWorldTransform(inputMap, src) + srcToWorld, err := sfs.GetFrameToWorldTransform(inputMap, src) if err != nil { return nil, err } From bee3296019691e4e7b21ff40d66fe03adb53ff59 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 22 Oct 2025 14:46:03 -0400 Subject: [PATCH 16/20] working --- motionplan/armplanning/real_test.go | 28 ++++++++++++++-------------- motionplan/armplanning/smart_seed.go | 3 ++- 2 files changed, 16 insertions(+), 15 deletions(-) diff --git a/motionplan/armplanning/real_test.go b/motionplan/armplanning/real_test.go index 091d3e86109..4c59e18e9aa 100644 --- a/motionplan/armplanning/real_test.go +++ b/motionplan/armplanning/real_test.go @@ -148,20 +148,20 @@ func TestSandingLargeMove1(t *testing.T) { } var pirIdealJointValues = [][]referenceframe.Input{ - {0 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {30 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {60 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {90 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {120 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {150 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {180 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {180 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {150 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {120 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {90 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {60 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {30 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {0 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {0 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {30 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {60 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {90 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {120 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {150 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {180 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {180 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {150 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {120 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {90 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {60 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {30 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {0 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, } func TestPirouette(t *testing.T) { diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index 78cbb36687e..6ae2501eb9b 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -311,7 +311,8 @@ func (ssc *smartSeedCache) findSeedsForFrame( return ret, nil } -func (ssc *smartSeedCache) addToCache(frameName string, frame referenceframe.Frame, inputs []float64) error { +func (ssc *smartSeedCache) addToCache(frameName string, frame referenceframe.Frame, inputsNotMine []float64) error { + inputs := append([]float64{}, inputsNotMine...) p, err := frame.Transform(inputs) if err != nil { return err From 1b1196ac280394aa02b61cc0fe5db4aa80f7356b Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 22 Oct 2025 15:45:39 -0400 Subject: [PATCH 17/20] cache frames not systems --- motionplan/armplanning/smart_seed.go | 162 +++++++++++++-------------- referenceframe/input.go | 7 -- 2 files changed, 81 insertions(+), 88 deletions(-) diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index 6ae2501eb9b..94314c62c2c 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -25,6 +25,21 @@ type goalCacheBox struct { entries []smartSeedCacheEntry } +func newCacheForFrame(f referenceframe.Frame) (*cacheForFrame, error) { + ccf := &cacheForFrame{} + + values := make([]float64, len(f.DoF())) + + err := ccf.buildCacheHelper(f, values, 0) + if err != nil { + return nil, fmt.Errorf("cannot buildCache for: %s: %w", f.Name(), err) + } + + ccf.buildInverseCache() + + return ccf, nil +} + type cacheForFrame struct { entries []smartSeedCacheEntry @@ -45,8 +60,43 @@ func (cff *cacheForFrame) boxKey(p r3.Vector) string { return fmt.Sprintf("%0.3d%0.3d%0.3d", x, y, z) } -func (cff *cacheForFrame) add(e smartSeedCacheEntry) { - cff.entries = append(cff.entries, e) +func (cff *cacheForFrame) buildCacheHelper(f referenceframe.Frame, values []float64, joint int) error { + limits := f.DoF() + + if joint > len(limits) { + panic(fmt.Errorf("joint: %d > len(limits): %d", joint, len(limits))) + } + + if joint == len(limits) { + return cff.addToCache(f, values) + } + + min, max, r := limits[joint].GoodLimits() + values[joint] = min + + jog := r / 10 + for values[joint] <= max { + err := cff.buildCacheHelper(f, values, joint+1) + if err != nil { + return err + } + + values[joint] += jog + } + + return nil +} + +func (cff *cacheForFrame) addToCache(frame referenceframe.Frame, inputsNotMine []float64) error { + inputs := append([]float64{}, inputsNotMine...) + p, err := frame.Transform(inputs) + if err != nil { + return err + } + + cff.entries = append(cff.entries, smartSeedCacheEntry{inputs, p}) + + return nil } func (cff *cacheForFrame) buildInverseCache() { @@ -109,8 +159,7 @@ func (cff *cacheForFrame) findBoxes(goalPose spatialmath.Pose) []*goalCacheBox { } type smartSeedCache struct { - fs *referenceframe.FrameSystem - lfs *linearizedFrameSystem + fs *referenceframe.FrameSystem rawCache map[string]*cacheForFrame } @@ -311,46 +360,14 @@ func (ssc *smartSeedCache) findSeedsForFrame( return ret, nil } -func (ssc *smartSeedCache) addToCache(frameName string, frame referenceframe.Frame, inputsNotMine []float64) error { - inputs := append([]float64{}, inputsNotMine...) - p, err := frame.Transform(inputs) - if err != nil { - return err - } - - ssc.rawCache[frameName].add(smartSeedCacheEntry{inputs, p}) - - return nil -} - -func (ssc *smartSeedCache) buildRawCache(frameName string, f referenceframe.Frame, values []float64, joint int) error { - limits := f.DoF() - - if joint > len(limits) { - panic(fmt.Errorf("joint: %d > len(limits): %d", joint, len(limits))) - } - - if joint == len(limits) { - return ssc.addToCache(frameName, f, values) - } - - min, max, r := limits[joint].GoodLimits() - values[joint] = min - - jog := r / 10 - for values[joint] <= max { - err := ssc.buildRawCache(frameName, f, values, joint+1) - if err != nil { - return err - } - - values[joint] += jog - } +var ( + sscCache = map[int]*cacheForFrame{} + sscCacheLock sync.Mutex +) - return nil -} +func (ssc *smartSeedCache) buildCacheForFrame(frameName string, logger logging.Logger) error { + var err error -func (ssc *smartSeedCache) buildCacheForFrame(frameName string) error { f := ssc.fs.Frame(frameName) if f == nil { return fmt.Errorf("no frame: %s", f) @@ -360,27 +377,38 @@ func (ssc *smartSeedCache) buildCacheForFrame(frameName string) error { return nil } - ssc.rawCache[frameName] = &cacheForFrame{} + hash := f.Hash() - values := make([]float64, len(f.DoF())) + sscCacheLock.Lock() + ccf, ok := sscCache[hash] + sscCacheLock.Unlock() - err := ssc.buildRawCache(frameName, f, values, 0) - if err != nil { - return fmt.Errorf("cannot buildCache for: %s: %w", frameName, err) + if !ok { + start := time.Now() + ccf, err = newCacheForFrame(f) + if err != nil { + return err + } + + logger.Infof("time to build: %v for: %v", time.Since(start), frameName) + + sscCacheLock.Lock() + sscCache[hash] = ccf + sscCacheLock.Unlock() } - ssc.rawCache[frameName].buildInverseCache() + ssc.rawCache[frameName] = ccf return nil } func (ssc *smartSeedCache) buildCache(logger logging.Logger) error { - logger.Debugf("buildCache %d %v", len(ssc.fs.FrameNames()), ssc.lfs.dof) + logger.Debugf("buildCache # of frames: %d", len(ssc.fs.FrameNames())) ssc.rawCache = map[string]*cacheForFrame{} for _, frameName := range ssc.fs.FrameNames() { - err := ssc.buildCacheForFrame(frameName) + err := ssc.buildCacheForFrame(frameName, logger) if err != nil { return fmt.Errorf("cannot build cache for frame: %s", frameName) } @@ -389,43 +417,15 @@ func (ssc *smartSeedCache) buildCache(logger logging.Logger) error { return nil } -var ( - sscCache = map[int]*smartSeedCache{} - sscCacheLock sync.Mutex -) - func smartSeed(fs *referenceframe.FrameSystem, logger logging.Logger) (*smartSeedCache, error) { - hash := fs.Hash() - var c *smartSeedCache - - sscCacheLock.Lock() - c, ok := sscCache[hash] - sscCacheLock.Unlock() - - if ok { - return c, nil - } - - lfs, err := newLinearizedFrameSystem(fs) - if err != nil { - return nil, err - } - - c = &smartSeedCache{ - fs: fs, - lfs: lfs, + c := &smartSeedCache{ + fs: fs, } - start := time.Now() - err = c.buildCache(logger) + err := c.buildCache(logger) if err != nil { return nil, err } - logger.Warnf("time to build: %v dof: %v rawCache size: %d hash: %v", time.Since(start), len(lfs.dof), len(c.rawCache), hash) - - sscCacheLock.Lock() - sscCache[hash] = c - sscCacheLock.Unlock() return c, nil } diff --git a/referenceframe/input.go b/referenceframe/input.go index deed710c2e2..f7f86347b7d 100644 --- a/referenceframe/input.go +++ b/referenceframe/input.go @@ -16,13 +16,6 @@ import ( // - prismatic inputs should be in mm. type Input = float64 -/* -// Hash returns a hash value for this input. -func (i *Input) Hash() int { - return (5 * (int(i.Value*100) + 1000)) * 2 -} -*/ - // JointPositionsFromInputs converts the given slice of Input to a JointPositions struct, // using the ProtobufFromInput function provided by the given Frame. func JointPositionsFromInputs(f Frame, inputs []Input) (*pb.JointPositions, error) { From 425d73dfe08c29349af8bab7265d2f4b6daaeca6 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 22 Oct 2025 17:36:13 -0400 Subject: [PATCH 18/20] many seeds --- motionplan/armplanning/cBiRRT.go | 2 +- motionplan/armplanning/node.go | 42 +++++++++------ motionplan/armplanning/smart_seed.go | 5 +- motionplan/baseplanning/cBiRRT.go | 2 +- motionplan/baseplanning/motion_planner.go | 2 +- motionplan/ik/combined.go | 6 +-- motionplan/ik/nlopt.go | 66 +++++++++++++++++------ motionplan/ik/nlopt_test.go | 4 +- motionplan/ik/solver.go | 12 +++-- motionplan/ik/solver_nocgo.go | 2 +- motionplan/ik/solver_test.go | 4 +- motionplan/tpspace/ptgIK.go | 2 +- 12 files changed, 97 insertions(+), 52 deletions(-) diff --git a/motionplan/armplanning/cBiRRT.go b/motionplan/armplanning/cBiRRT.go index ede9ba47979..315c36f2df9 100644 --- a/motionplan/armplanning/cBiRRT.go +++ b/motionplan/armplanning/cBiRRT.go @@ -293,7 +293,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear( return nil } - solutions, err := ik.DoSolve(ctx, mp.fastGradDescent, mp.pc.linearizeFSmetric(mp.psc.checker.PathMetric()), linearSeed, .25) + solutions, err := ik.DoSolve(ctx, mp.fastGradDescent, mp.pc.linearizeFSmetric(mp.psc.checker.PathMetric()), [][]float64{linearSeed}, .25) if err != nil { mp.pc.logger.Debugf("constrainNear fail (DoSolve): %v", err) return nil diff --git a/motionplan/armplanning/node.go b/motionplan/armplanning/node.go index d63c6c87b31..bfc6bddffca 100644 --- a/motionplan/armplanning/node.go +++ b/motionplan/armplanning/node.go @@ -107,8 +107,9 @@ type solutionSolvingState struct { psc *planSegmentContext maxSolutions int - seed referenceframe.FrameSystemInputs - linearSeed []float64 + seeds []referenceframe.FrameSystemInputs + linearSeeds [][]float64 + moving, nonmoving []string ratios []float64 @@ -130,7 +131,7 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er sss := &solutionSolvingState{ psc: psc, - seed: psc.start, + seeds: []referenceframe.FrameSystemInputs{psc.start}, solutions: []*node{}, failures: newIkConstraintError(psc.pc.fs, psc.checker), startTime: time.Now(), @@ -144,22 +145,31 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er sss.maxSolutions = defaultSolutionsToSeed } - if len(psc.pc.lfs.dof) <= 6 { + ls, err := psc.pc.lfs.mapToSlice(psc.start) + if err != nil { + return nil, err + } + sss.linearSeeds = [][]float64{ls} + + if len(psc.pc.lfs.dof) <= 6 { // TODO - remove the limit ssc, err := smartSeed(psc.pc.fs, psc.pc.logger) if err != nil { - return nil, err + return nil, fmt.Errorf("cannot create smartSeeder: %w", err) } - sss.seed, err = ssc.findSeed(psc.goal, psc.start, psc.pc.logger) + altSeeds, err := ssc.findSeeds(psc.goal, psc.start, psc.pc.logger) if err != nil { - return nil, err + psc.pc.logger.Warnf("findSeeds failed, ignoring: %v", err) + } + for _, s := range altSeeds { + ls, err := psc.pc.lfs.mapToSlice(s) + if err != nil { + psc.pc.logger.Warnf("mapToSlice failed? %v", err) + continue + } + sss.seeds = append(sss.seeds, s) + sss.linearSeeds = append(sss.linearSeeds, ls) } - } - - psc.pc.logger.Debugf("psc.start -> seed: \n%v\n%v", psc.start, sss.seed) - sss.linearSeed, err = psc.pc.lfs.mapToSlice(sss.seed) - if err != nil { - return nil, err } sss.moving, sss.nonmoving = sss.psc.motionChains.framesFilteredByMovingAndNonmoving() @@ -173,12 +183,12 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er } func (sss *solutionSolvingState) computeGoodCost(goal referenceframe.FrameSystemPoses) error { - sss.ratios = sss.psc.pc.lfs.inputChangeRatio(sss.psc.motionChains, sss.seed, + sss.ratios = sss.psc.pc.lfs.inputChangeRatio(sss.psc.motionChains, sss.seeds[0], /* maybe use the best one? */ sss.psc.pc.planOpts.getGoalMetric(goal), sss.psc.pc.logger) adjusted := []float64{} for idx, r := range sss.ratios { - adjusted = append(adjusted, sss.psc.pc.lfs.jog(idx, sss.linearSeed[idx], r)) + adjusted = append(adjusted, sss.psc.pc.lfs.jog(idx, sss.linearSeeds[0][idx] /* match above when we change */, r)) } step, err := sss.psc.pc.lfs.sliceToMap(adjusted) if err != nil { @@ -399,7 +409,7 @@ func getSolutions(ctx context.Context, psc *planSegmentContext) ([]*node, error) utils.PanicCapturingGo(func() { // This channel close doubles as signaling that the goroutine has exited. defer close(solutionGen) - _, err := solver.Solve(ctxWithCancel, solutionGen, solvingState.linearSeed, solvingState.ratios, minFunc, psc.pc.randseed.Int()) + _, err := solver.Solve(ctxWithCancel, solutionGen, solvingState.linearSeeds, solvingState.ratios, minFunc, psc.pc.randseed.Int()) if err != nil { solveErrorLock.Lock() solveError = err diff --git a/motionplan/armplanning/smart_seed.go b/motionplan/armplanning/smart_seed.go index 94314c62c2c..5599a53ed3d 100644 --- a/motionplan/armplanning/smart_seed.go +++ b/motionplan/armplanning/smart_seed.go @@ -229,7 +229,7 @@ func (ssc *smartSeedCache) findSeed(goal referenceframe.FrameSystemPoses, return nil, err } if len(ss) == 0 { - return nil, fmt.Errorf("no findSeeds results") + return start, nil } return ss[0], nil } @@ -325,8 +325,7 @@ func (ssc *smartSeedCache) findSeedsForFrame( } if len(best) == 0 { - logger.Debugf("no best, returning start") - return [][]referenceframe.Input{start}, nil + return nil, nil } sort.Slice(best, func(i, j int) bool { diff --git a/motionplan/baseplanning/cBiRRT.go b/motionplan/baseplanning/cBiRRT.go index d93ef893abb..da4b467b8f6 100644 --- a/motionplan/baseplanning/cBiRRT.go +++ b/motionplan/baseplanning/cBiRRT.go @@ -364,7 +364,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear( } // Spawn the IK solver to generate solutions until done - _, err = mp.fastGradDescent.Solve(ctx, solutionGen, linearSeed, nil, + _, err = mp.fastGradDescent.Solve(ctx, solutionGen, [][]float64{linearSeed}, nil, mp.linearizeFSmetric(mp.ConstraintChecker.PathMetric()), randseed.Int()) // We should have zero or one solutions var solved *ik.Solution diff --git a/motionplan/baseplanning/motion_planner.go b/motionplan/baseplanning/motion_planner.go index 44b1207c4c8..bcc64ef47c1 100644 --- a/motionplan/baseplanning/motion_planner.go +++ b/motionplan/baseplanning/motion_planner.go @@ -354,7 +354,7 @@ func (mp *planner) getSolutions( utils.PanicCapturingGo(func() { defer activeSolvers.Done() defer solverFinished.Store(true) - _, err := mp.solver.Solve(ctxWithCancel, solutionGen, linearSeed, ratios, minFunc, mp.randseed.Int()) + _, err := mp.solver.Solve(ctxWithCancel, solutionGen, [][]float64{linearSeed}, ratios, minFunc, mp.randseed.Int()) if err != nil { if ctxWithCancel.Err() == nil { mp.logger.Warnf("solver had an error: %v", err) diff --git a/motionplan/ik/combined.go b/motionplan/ik/combined.go index 369669cda33..da4ba497ead 100644 --- a/motionplan/ik/combined.go +++ b/motionplan/ik/combined.go @@ -49,7 +49,7 @@ func CreateCombinedIKSolver( // positions. If unable to solve, the returned error will be non-nil. func (ik *CombinedIK) Solve(ctx context.Context, retChan chan<- *Solution, - seed []float64, + seeds [][]float64, travelPercent []float64, costFunc CostFunc, rseed int, @@ -69,7 +69,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, rseed += 1500 parseed := rseed thisSolver := solver - seedFloats := seed + seedFloats := seeds var myTravelPercent []float64 if bottomThird(i, len(ik.solvers)) { @@ -79,7 +79,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, } else if middleThird(i, len(ik.solvers)) { myTravelPercent = travelPercent } else { - seedFloats = generateRandomPositions(randSeed, lowerBound, upperBound) + seedFloats = [][]float64{generateRandomPositions(randSeed, lowerBound, upperBound)} } activeSolvers.Add(1) diff --git a/motionplan/ik/nlopt.go b/motionplan/ik/nlopt.go index d0678685c1a..e05533124c7 100644 --- a/motionplan/ik/nlopt.go +++ b/motionplan/ik/nlopt.go @@ -66,39 +66,46 @@ func (ik *NloptIK) DoF() []referenceframe.Limit { return ik.limits } +func (ik *NloptIK) computeLimits(seed, travelPercent []float64) ([]float64, []float64) { + lowerBound, upperBound := limitsToArrays(ik.limits) + + if len(travelPercent) == len(lowerBound) { + for i := 0; i < len(lowerBound); i++ { + lowerBound[i] = max(lowerBound[i], seed[i]-(ik.limits[i].Range()*travelPercent[i])) + upperBound[i] = min(upperBound[i], seed[i]+(ik.limits[i].Range()*travelPercent[i])) + } + } + + return lowerBound, upperBound +} + // Solve runs the actual solver and sends any solutions found to the given channel. func (ik *NloptIK) Solve(ctx context.Context, solutionChan chan<- *Solution, - seed []float64, + seeds [][]float64, travelPercent []float64, minFunc CostFunc, rseed int, ) (int, error) { - if len(seed) != len(ik.limits) { - return 0, fmt.Errorf("nlopt initialized with %d dof but seed was length %d", len(ik.limits), len(seed)) + if len(seeds) == 0 { + return 0, fmt.Errorf("no seeds") + } + + if len(seeds[0]) != len(ik.limits) { + return 0, fmt.Errorf("nlopt initialized with %d dof but seed was length %d", len(ik.limits), len(seeds[0])) } //nolint: gosec randSeed := rand.New(rand.NewSource(int64(rseed))) var err error // Determine optimal jump values; start with default, and if gradient is zero, increase to 1 to try to avoid underflow. - jump := ik.calcJump(ctx, defaultJump, seed, minFunc) - - iterations := 0 - solutionsFound := 0 + jump := ik.calcJump(ctx, defaultJump, seeds[0], minFunc) - lowerBound, upperBound := limitsToArrays(ik.limits) + lowerBound, upperBound := ik.computeLimits(seeds[0], travelPercent) if len(lowerBound) == 0 || len(upperBound) == 0 { return 0, errBadBounds } - if len(travelPercent) == len(lowerBound) { - for i := 0; i < len(lowerBound); i++ { - lowerBound[i] = max(lowerBound[i], seed[i]-(ik.limits[i].Range()*travelPercent[i])) - upperBound[i] = min(upperBound[i], seed[i]+(ik.limits[i].Range()*travelPercent[i])) - } - } - opt, err := nlopt.NewNLopt(nlopt.LD_SLSQP, uint(len(lowerBound))) defer opt.Destroy() if err != nil { @@ -106,6 +113,9 @@ func (ik *NloptIK) Solve(ctx context.Context, } jumpVal := 0. + iterations := 0 + solutionsFound := 0 + // checkVals is our set of inputs that we evaluate for distance // Gradient is, under the hood, a unsafe C structure that we are meant to mutate in place. nloptMinFunc := func(checkVals, gradient []float64) float64 { @@ -158,6 +168,8 @@ func (ik *NloptIK) Solve(ctx context.Context, } } + seedNumber := 0 + seed := seeds[0] for iterations < ik.maxIterations { if ctx.Err() != nil { break @@ -191,7 +203,29 @@ func (ik *NloptIK) Solve(ctx context.Context, } } - seed = generateRandomPositions(randSeed, lowerBound, upperBound) + changedBounds := false + seedNumber++ + if seedNumber < len(seeds) { + seed = seeds[seedNumber] + lowerBound, upperBound = ik.computeLimits(seed, travelPercent) + changedBounds = true + } else if seedNumber == len(seeds) { + lowerBound, upperBound = ik.computeLimits(seeds[0], travelPercent) + seed = generateRandomPositions(randSeed, lowerBound, upperBound) + changedBounds = true + } else { + seed = generateRandomPositions(randSeed, lowerBound, upperBound) + } + + if changedBounds { + err = multierr.Combine( + opt.SetLowerBounds(lowerBound), + opt.SetUpperBounds(upperBound), + ) + if err != nil { + return 0, err + } + } } return solutionsFound, nil diff --git a/motionplan/ik/nlopt_test.go b/motionplan/ik/nlopt_test.go index 2c46a39078d..3f43e9fb57d 100644 --- a/motionplan/ik/nlopt_test.go +++ b/motionplan/ik/nlopt_test.go @@ -26,7 +26,7 @@ func TestCreateNloptSolver(t *testing.T) { pos := spatialmath.NewPoseFromPoint(r3.Vector{X: 207, Z: 112}) seed := []float64{1, 1, -1, 1, 1, 0} solveFunc := NewMetricMinFunc(motionplan.NewSquaredNormMetric(pos), m, logger) - _, err = DoSolve(context.Background(), ik, solveFunc, seed, 1) + _, err = DoSolve(context.Background(), ik, solveFunc, [][]float64{seed}, 1) test.That(t, err, test.ShouldBeNil) pos = spatialmath.NewPose( @@ -38,6 +38,6 @@ func TestCreateNloptSolver(t *testing.T) { seed = m.InputFromProtobuf(&pb.JointPositions{Values: []float64{49, 28, -101, 0, -73, 0}}) solveFunc = NewMetricMinFunc(motionplan.NewSquaredNormMetric(pos), m, logger) - _, err = DoSolve(context.Background(), ik, solveFunc, seed, 1) + _, err = DoSolve(context.Background(), ik, solveFunc, [][]float64{seed}, 1) test.That(t, err, test.ShouldBeNil) } diff --git a/motionplan/ik/solver.go b/motionplan/ik/solver.go index 0281f1dca0e..266a9e6cffe 100644 --- a/motionplan/ik/solver.go +++ b/motionplan/ik/solver.go @@ -34,7 +34,7 @@ type Solver interface { referenceframe.Limited // Solve receives a context, a channel to which solutions will be provided, a function whose output should be minimized, and a // number of iterations to run. - Solve(ctx context.Context, solutions chan<- *Solution, seed []float64, + Solve(ctx context.Context, solutions chan<- *Solution, seeds [][]float64, travelPercent []float64, minFunc CostFunc, rseed int) (int, error) } @@ -96,11 +96,13 @@ func NewMetricMinFunc(metric motionplan.StateMetric, frame referenceframe.Frame, // // but will fail if you have to move. 1 means search the entire range. func DoSolve(ctx context.Context, solver Solver, solveFunc CostFunc, - seed []float64, rangeModifier float64, + seeds [][]float64, rangeModifier float64, ) ([][]float64, error) { travelPercent := []float64{} - for range seed { - travelPercent = append(travelPercent, rangeModifier) + if len(seeds) > 0 { + for range seeds[0] { + travelPercent = append(travelPercent, rangeModifier) + } } solutionGen := make(chan *Solution) @@ -109,7 +111,7 @@ func DoSolve(ctx context.Context, solver Solver, solveFunc CostFunc, go func() { defer close(solutionGen) - _, err := solver.Solve(ctx, solutionGen, seed, travelPercent, solveFunc, 1) + _, err := solver.Solve(ctx, solutionGen, seeds, travelPercent, solveFunc, 1) solveErrors = err }() diff --git a/motionplan/ik/solver_nocgo.go b/motionplan/ik/solver_nocgo.go index 322f1d34d51..12671ed2820 100644 --- a/motionplan/ik/solver_nocgo.go +++ b/motionplan/ik/solver_nocgo.go @@ -27,7 +27,7 @@ type NloptIK struct{} // Solve refuses to solve problems without cgo. func (ik *NloptIK) Solve(ctx context.Context, solutionChan chan<- *Solution, - seed []float64, + seeds [][]float64, travelPercent []float64, minFunc CostFunc, rseed int, diff --git a/motionplan/ik/solver_test.go b/motionplan/ik/solver_test.go index 31e301d6490..8b8549ee20a 100644 --- a/motionplan/ik/solver_test.go +++ b/motionplan/ik/solver_test.go @@ -17,7 +17,7 @@ import ( ) var ( - home = []float64{0, 0, 0, 0, 0, 0} + home = [][]float64{{0, 0, 0, 0, 0, 0}} nCPU = int(math.Max(1.0, float64(runtime.NumCPU()/4))) ) @@ -43,7 +43,7 @@ func TestCombinedIKinematics(t *testing.T) { &spatial.OrientationVectorDegrees{OX: 1.78, OY: -3.3, OZ: -1.11}, ) solveFunc = NewMetricMinFunc(motionplan.NewSquaredNormMetric(pos), m, logger) - _, err = DoSolve(context.Background(), ik, solveFunc, solution[0], 1) + _, err = DoSolve(context.Background(), ik, solveFunc, solution, 1) test.That(t, err, test.ShouldBeNil) } diff --git a/motionplan/tpspace/ptgIK.go b/motionplan/tpspace/ptgIK.go index 8e418ab612b..270637e987f 100644 --- a/motionplan/tpspace/ptgIK.go +++ b/motionplan/tpspace/ptgIK.go @@ -99,7 +99,7 @@ func (ptg *ptgIK) Solve( _, err := ptg.fastGradDescent.Solve( ctx, internalSolutionGen, - seed, + [][]float64{seed}, nil, ptg.ptgMetricIkFunc(solveMetric), defaultNloptSeed, From 62ca08b1f3bbaf99cc41437c036b9a0f1d3b0149 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 22 Oct 2025 17:54:41 -0400 Subject: [PATCH 19/20] fix 32bit --- motionplan/armplanning/real_test.go | 17 ----------------- motionplan/armplanning/smart_seed_test.go | 17 +++++++++++++++++ 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/motionplan/armplanning/real_test.go b/motionplan/armplanning/real_test.go index 4c59e18e9aa..ddc738f76d2 100644 --- a/motionplan/armplanning/real_test.go +++ b/motionplan/armplanning/real_test.go @@ -147,23 +147,6 @@ func TestSandingLargeMove1(t *testing.T) { test.That(t, len(solution.steps), test.ShouldEqual, 1) } -var pirIdealJointValues = [][]referenceframe.Input{ - {0 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {30 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {60 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {90 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {120 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {150 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {180 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {180 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {150 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {120 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {90 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {60 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {30 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, - {0 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, -} - func TestPirouette(t *testing.T) { // get arm kinematics for forward kinematics armName := "ur5e" diff --git a/motionplan/armplanning/smart_seed_test.go b/motionplan/armplanning/smart_seed_test.go index 66409393b77..78233fe95bb 100644 --- a/motionplan/armplanning/smart_seed_test.go +++ b/motionplan/armplanning/smart_seed_test.go @@ -13,6 +13,23 @@ import ( "go.viam.com/rdk/utils" ) +var pirIdealJointValues = [][]referenceframe.Input{ + {0 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {30 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {60 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {90 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {120 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {150 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {180 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {180 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {150 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {120 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {90 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {60 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {30 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, + {0 * 3.1415 / 180.0, 0, -90 * 3.1415 / 180.0, 0, 0, 0}, +} + func TestSmartSeedCache1(t *testing.T) { logger := logging.NewTestLogger(t) From dfda625ef466076edea3b52e994c31cba1aa9419 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Wed, 22 Oct 2025 19:03:38 -0400 Subject: [PATCH 20/20] extend test timeout --- etc/test.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/etc/test.sh b/etc/test.sh index 28dc0b086f8..461b33dfeaa 100755 --- a/etc/test.sh +++ b/etc/test.sh @@ -25,7 +25,7 @@ if test -n "$GITHUB_RUN_ID"; then fi # We run analyzetests on every run, pass or fail. We only run analyzecoverage when all tests passed. -PION_LOG_WARN=webrtc,datachannel,sctp gotestsum --format $FORMAT $LOGFILE -- -tags=no_skip $RACE $COVER $TEST_TARGET +PION_LOG_WARN=webrtc,datachannel,sctp gotestsum --format $FORMAT $LOGFILE -- -tags=no_skip -timeout 30m $RACE $COVER $TEST_TARGET SUCCESS=$? if [[ $RACE != "" ]]; then