Skip to content

20251015 ik cache #5380

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 25 commits into from
Oct 22, 2025
Merged

20251015 ik cache #5380

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
25 commits
Select commit Hold shift + click to select a range
26343ae
wip
erh Oct 15, 2025
8ade117
wip2
erh Oct 15, 2025
dcdd4cf
wip
erh Oct 15, 2025
9777fc5
a mess
erh Oct 15, 2025
98ae329
Merge remote-tracking branch 'origin/main' into 20251015-ik-cache
erh Oct 15, 2025
b694644
lint
erh Oct 15, 2025
230bfff
turn off smarts
erh Oct 16, 2025
81a60c3
Merge remote-tracking branch 'origin/main' into 20251015-ik-cache
erh Oct 16, 2025
aefb594
Merge branch 'main' into 20251015-ik-cache
erh Oct 16, 2025
1b96aba
before merge conflict
erh Oct 19, 2025
dd92794
Merge remote-tracking branch 'origin/main' into 20251015-ik-cache
erh Oct 19, 2025
774894c
put back
erh Oct 19, 2025
c372fce
broken wip
erh Oct 20, 2025
01d7ea7
better
erh Oct 20, 2025
c1bd866
change name
erh Oct 21, 2025
3fbfcee
compiles
erh Oct 22, 2025
d55135e
lint
erh Oct 22, 2025
59f1e3f
closer
erh Oct 22, 2025
fd2a838
working
erh Oct 22, 2025
64c382a
Merge remote-tracking branch 'origin/main' into 20251015-ik-cache
erh Oct 22, 2025
bee3296
working
erh Oct 22, 2025
1b1196a
cache frames not systems
erh Oct 22, 2025
425d73d
many seeds
erh Oct 22, 2025
62ca08b
fix 32bit
erh Oct 22, 2025
dfda625
extend test timeout
erh Oct 22, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion etc/test.sh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion motionplan/armplanning/cBiRRT.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
41 changes: 33 additions & 8 deletions motionplan/armplanning/node.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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(),
Expand All @@ -144,11 +145,32 @@ func newSolutionSolvingState(psc *planSegmentContext) (*solutionSolvingState, er
sss.maxSolutions = defaultSolutionsToSeed
}

psc.pc.logger.Debugf("psc.start -> seed: \n%v\n%v", psc.start, sss.seed)
sss.linearSeed, err = psc.pc.lfs.mapToSlice(sss.seed)
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, fmt.Errorf("cannot create smartSeeder: %w", err)
}

altSeeds, err := ssc.findSeeds(psc.goal, psc.start, psc.pc.logger)
if err != nil {
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)
}
}

sss.moving, sss.nonmoving = sss.psc.motionChains.framesFilteredByMovingAndNonmoving()

Expand All @@ -161,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 {
Expand Down Expand Up @@ -314,6 +336,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
Expand Down Expand Up @@ -384,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
Expand Down
18 changes: 2 additions & 16 deletions motionplan/armplanning/real_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -153,22 +153,8 @@ func TestPirouette(t *testing.T) {
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
Expand Down
Loading
Loading