Skip to content

Commit

Permalink
RSDK-9076 make ik functions more generic (#4467)
Browse files Browse the repository at this point in the history
  • Loading branch information
biotinker authored Oct 24, 2024
1 parent 597e6cb commit b9d8078
Show file tree
Hide file tree
Showing 14 changed files with 292 additions and 355 deletions.
12 changes: 2 additions & 10 deletions components/base/kinematicbase/execution.go
Original file line number Diff line number Diff line change
Expand Up @@ -495,7 +495,6 @@ func (ptgk *ptgBaseKinematics) courseCorrect(
func (ptgk *ptgBaseKinematics) getCorrectionSolution(ctx context.Context, goals []courseCorrectionGoal) (courseCorrectionGoal, error) {
for _, goal := range goals {
solveMetric := ik.NewScaledSquaredNormMetric(goal.Goal, 50)
solutionChan := make(chan *ik.Solution, 1)
ptgk.logger.Debug("attempting goal ", spatialmath.PoseToProtobuf(goal.Goal))
seed := []referenceframe.Input{{math.Pi / 2}, {ptgk.linVelocityMMPerSecond / 2}, {math.Pi / 2}, {ptgk.linVelocityMMPerSecond / 2}}
if goal.Goal.Point().X > 0 {
Expand All @@ -505,24 +504,17 @@ func (ptgk *ptgBaseKinematics) getCorrectionSolution(ctx context.Context, goals
}
// Attempt to use our course correction solver to solve for a new set of trajectories which will get us from our current position
// to our goal point along our original trajectory.
err := ptgk.ptgs[ptgk.courseCorrectionIdx].Solve(
solution, err := ptgk.ptgs[ptgk.courseCorrectionIdx].Solve(
ctx,
solutionChan,
seed,
solveMetric,
0,
)
if err != nil {
return courseCorrectionGoal{}, err
}
var solution *ik.Solution
select {
case solution = <-solutionChan:
default:
}
ptgk.logger.Debug("solution ", solution)
if solution.Score < courseCorrectionMaxScore {
goal.Solution = solution.Configuration
goal.Solution = referenceframe.FloatsToInputs(solution.Configuration)
return goal, nil
}
}
Expand Down
23 changes: 18 additions & 5 deletions motionplan/cBiRRT.go
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ func newCbirrtOptions(planOpts *plannerOptions) (*cbirrtOptions, error) {
// https://ieeexplore.ieee.org/document/5152399/
type cBiRRTMotionPlanner struct {
*planner
fastGradDescent *ik.NloptIK
fastGradDescent ik.Solver
algOpts *cbirrtOptions
}

Expand All @@ -75,7 +75,7 @@ func newCBiRRTMotionPlanner(
return nil, err
}
// nlopt should try only once
nlopt, err := ik.CreateNloptIKSolver(frame, logger, 1, true, true)
nlopt, err := ik.CreateNloptSolver(frame.DoF(), logger, 1, true, true)
if err != nil {
return nil, err
}
Expand Down Expand Up @@ -372,7 +372,16 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
}
solutionGen := make(chan *ik.Solution, 1)
// Spawn the IK solver to generate solutions until done
err = mp.fastGradDescent.Solve(ctx, solutionGen, target, mp.planOpts.pathMetric, randseed.Int())
err = ik.SolveMetric(
ctx,
mp.fastGradDescent,
mp.frame,
solutionGen,
target,
mp.planOpts.pathMetric,
randseed.Int(),
mp.logger,
)
// We should have zero or one solutions
var solved *ik.Solution
select {
Expand All @@ -385,11 +394,15 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
}

ok, failpos := mp.planOpts.CheckSegmentAndStateValidity(
&ik.Segment{StartConfiguration: seedInputs, EndConfiguration: solved.Configuration, Frame: mp.frame},
&ik.Segment{
StartConfiguration: seedInputs,
EndConfiguration: referenceframe.FloatsToInputs(solved.Configuration),
Frame: mp.frame,
},
mp.planOpts.Resolution,
)
if ok {
return solved.Configuration
return referenceframe.FloatsToInputs(solved.Configuration)
}
if failpos != nil {
dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: target, EndConfiguration: failpos.EndConfiguration})
Expand Down
57 changes: 34 additions & 23 deletions motionplan/ik/combinedInverseKinematics.go
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@ package ik

import (
"context"
"math/rand"
"sync"

"go.uber.org/multierr"
Expand All @@ -13,24 +14,30 @@ import (
"go.viam.com/rdk/referenceframe"
)

// CombinedIK defines the fields necessary to run a combined solver.
type CombinedIK struct {
solvers []InverseKinematics
model referenceframe.Frame
// combinedIK defines the fields necessary to run a combined solver.
type combinedIK struct {
solvers []Solver
logger logging.Logger
limits []referenceframe.Limit
}

// CreateCombinedIKSolver creates a combined parallel IK solver with a number of nlopt solvers equal to the nCPU
// passed in. Each will be given a different random seed. When asked to solve, all solvers will be run in parallel
// CreateCombinedIKFrameSolver creates a combined parallel IK solver that operates on a frame with a number of nlopt solvers equal to the
// nCPU passed in. Each will be given a different random seed. When asked to solve, all solvers will be run in parallel
// and the first valid found solution will be returned.
func CreateCombinedIKSolver(model referenceframe.Frame, logger logging.Logger, nCPU int, goalThreshold float64) (*CombinedIK, error) {
ik := &CombinedIK{}
ik.model = model
func CreateCombinedIKFrameSolver(
model referenceframe.Frame,
logger logging.Logger,
nCPU int,
goalThreshold float64,
) (Solver, error) {
ik := &combinedIK{}
ik.limits = model.DoF()
if nCPU == 0 {
nCPU = 1
}
for i := 1; i <= nCPU; i++ {
nlopt, err := CreateNloptIKSolver(model, logger, -1, true, true)
solver, err := CreateNloptSolver(ik.limits, logger, -1, true, true)
nlopt := solver.(*nloptIK)
nlopt.id = i
if err != nil {
return nil, err
Expand All @@ -43,30 +50,39 @@ func CreateCombinedIKSolver(model referenceframe.Frame, logger logging.Logger, n

// Solve will initiate solving for the given position in all child solvers, seeding with the specified initial joint
// positions. If unable to solve, the returned error will be non-nil.
func (ik *CombinedIK) Solve(ctx context.Context,
func (ik *combinedIK) Solve(ctx context.Context,
c chan<- *Solution,
seed []referenceframe.Input,
m StateMetric,
seed []float64,
m func([]float64) float64,
rseed int,
) error {
var err error
ctxWithCancel, cancel := context.WithCancel(ctx)
defer cancel()

//nolint: gosec
randSeed := rand.New(rand.NewSource(int64(rseed)))

errChan := make(chan error, len(ik.solvers))
var activeSolvers sync.WaitGroup
defer activeSolvers.Wait()
activeSolvers.Add(len(ik.solvers))

for _, solver := range ik.solvers {
lowerBound, upperBound := limitsToArrays(ik.limits)

for i, solver := range ik.solvers {
rseed += 1500
parseed := rseed
thisSolver := solver
seedFloats := seed
if i > 0 {
seedFloats = generateRandomPositions(randSeed, lowerBound, upperBound)
}

utils.PanicCapturingGo(func() {
defer activeSolvers.Done()

errChan <- thisSolver.Solve(ctxWithCancel, c, seed, m, parseed)
errChan <- thisSolver.Solve(ctxWithCancel, c, seedFloats, m, parseed)
})
}

Expand Down Expand Up @@ -117,12 +133,7 @@ func (ik *CombinedIK) Solve(ctx context.Context,
return collectedErrs
}

// Frame returns the associated referenceframe.
func (ik *CombinedIK) Frame() referenceframe.Frame {
return ik.model
}

// DoF returns the DoF of the associated referenceframe.
func (ik *CombinedIK) DoF() []referenceframe.Limit {
return ik.model.DoF()
// DoF returns the DoF of the solver.
func (ik *combinedIK) DoF() []referenceframe.Limit {
return ik.limits
}
33 changes: 0 additions & 33 deletions motionplan/ik/inverseKinematics.go

This file was deleted.

Loading

0 comments on commit b9d8078

Please sign in to comment.