Skip to content

[RSDK-11154] draft NOT TO BE MERGED #5188

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

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

[RSDK-11154] draft NOT TO BE MERGED #5188

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
22 changes: 11 additions & 11 deletions motionplan/armplanning/motionPlanner.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -563,8 +563,8 @@ func (mp *planner) getSolutions(
failures := map[string]int{}
constraintFailCnt := 0

startTime := time.Now()
firstSolutionTime := time.Hour
// startTime := time.Now()
// firstSolutionTime := time.Hour

// Solve the IK solver. Loop labels are required because `break` etc in a `select` will break only the `select`.
IK:
Expand Down Expand Up @@ -625,15 +625,15 @@ IK:
break IK
}

if len(solutions) == 1 {
firstSolutionTime = time.Since(startTime)
} else {
elapsed := time.Since(startTime)
if elapsed > (time.Duration(mp.planOpts.TimeMultipleAfterFindingFirstSolution) * firstSolutionTime) {
mp.logger.Infof("ending early because of time elapsed: %v firstSolutionTime: %v", elapsed, firstSolutionTime)
break IK
}
}
// if len(solutions) == 1 {
// firstSolutionTime = time.Since(startTime)
// } else {
// elapsed := time.Since(startTime)
// if elapsed > (time.Duration(mp.planOpts.TimeMultipleAfterFindingFirstSolution) * firstSolutionTime) {
// mp.logger.Infof("ending early because of time elapsed: %v firstSolutionTime: %v", elapsed, firstSolutionTime)
// break IK
// }
// }
} else {
constraintFailCnt++
failures[err.Error()]++
Expand Down
13 changes: 6 additions & 7 deletions motionplan/armplanning/plannerOptions.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,13 +66,12 @@ const (
// var defaultPlanner = newCBiRRTMotionPlanner.
)

var (
defaultNumThreads = utils.MinInt(runtime.NumCPU()/2, 10)
defaultTimeMultipleAfterFindingFirstSolution = 10
)
var defaultNumThreads = runtime.NumCPU() / 2

// defaultTimeMultipleAfterFindingFirstSolution = 10

func init() {
defaultTimeMultipleAfterFindingFirstSolution = utils.GetenvInt("MP_TIME_MULTIPLIER", defaultTimeMultipleAfterFindingFirstSolution)
// defaultTimeMultipleAfterFindingFirstSolution = utils.GetenvInt("MP_TIME_MULTIPLIER", defaultTimeMultipleAfterFindingFirstSolution)
defaultNumThreads = utils.GetenvInt("MP_NUM_THREADS", defaultNumThreads)
}

Expand Down Expand Up @@ -115,7 +114,7 @@ func NewBasicPlannerOptions() *PlannerOptions {

opt.SmoothIter = defaultSmoothIter

opt.TimeMultipleAfterFindingFirstSolution = defaultTimeMultipleAfterFindingFirstSolution
// opt.TimeMultipleAfterFindingFirstSolution = defaultTimeMultipleAfterFindingFirstSolution
opt.NumThreads = defaultNumThreads

opt.LineTolerance = defaultLinearDeviation
Expand Down Expand Up @@ -237,7 +236,7 @@ type PlannerOptions struct {
// For inverse kinematics, the time within which each pending solution must finish its computation is
// a multiple of the time taken to compute the first solution. This parameter is a way to
// set that multiplicative factor.
TimeMultipleAfterFindingFirstSolution int `json:"time_multiple_after_finding_first_solution"`
// TimeMultipleAfterFindingFirstSolution int `json:"time_multiple_after_finding_first_solution"`
}

// NewPlannerOptionsFromExtra returns basic default settings updated by overridden parameters
Expand Down
Loading