-
Notifications
You must be signed in to change notification settings - Fork 110
/
plannerOptions.go
136 lines (105 loc) · 4.64 KB
/
plannerOptions.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
package motionplan
import (
"math"
"gonum.org/v1/gonum/floats"
)
// default values for planning options.
const (
// max linear deviation from straight-line between start and goal, in mm.
defaultLinearDeviation = 0.1
// allowable deviation from slerp between start/goal orientations, unit is the norm of the R3AA between start and goal.
defaultOrientationDeviation = 0.05
// allowable linear and orientation deviation from direct interpolation path, as a proportion of the linear and orientation distances
// between the start and goal.
defaultPseudolinearTolerance = 0.8
// Number of IK solutions that should be generated before stopping.
defaultSolutionsToSeed = 50
// Check constraints are still met every this many mm/degrees of movement.
defaultResolution = 2.0
// If an IK solution scores below this much, return it immediately.
defaultMinIkScore = 0.
// Default distance below which two distances are considered equal.
defaultEpsilon = 0.001
// default number of seconds to try to solve in total before returning.
defaultTimeout = 300.
// names of constraints.
defaultLinearConstraintName = "defaultLinearConstraint"
defaultPseudolinearConstraintName = "defaultPseudolinearConstraint"
defaultOrientationConstraintName = "defaultOrientationConstraint"
defaultCollisionConstraintName = "defaultCollisionConstraint"
defaultJointConstraint = "defaultJointSwingConstraint"
// When breaking down a path into smaller waypoints, add a waypoint every this many mm of movement.
defaultPathStepSize = 10
)
var defaultPlanner = newCBiRRTMotionPlanner
// the set of supported motion profiles.
const (
FreeMotionProfile = "free"
LinearMotionProfile = "linear"
PseudolinearMotionProfile = "pseudolinear"
OrientationMotionProfile = "orientation"
PositionOnlyMotionProfile = "position_only"
)
// defaultDistanceFunc returns the square of the two-norm between the StartInput and EndInput vectors in the given ConstraintInput.
func defaultDistanceFunc(ci *ConstraintInput) (bool, float64) {
diff := make([]float64, 0, len(ci.StartInput))
for i, f := range ci.StartInput {
diff = append(diff, f.Value-ci.EndInput[i].Value)
}
// 2 is the L value returning a standard L2 Normalization
return true, floats.Norm(diff, 2)
}
// NewBasicPlannerOptions specifies a set of basic options for the planner.
func newBasicPlannerOptions() *plannerOptions {
opt := &plannerOptions{}
opt.AddConstraint(defaultJointConstraint, NewJointConstraint(math.Inf(1)))
opt.metric = NewSquaredNormMetric()
opt.pathDist = NewSquaredNormMetric()
// Set defaults
opt.MaxSolutions = defaultSolutionsToSeed
opt.MinScore = defaultMinIkScore
opt.Resolution = defaultResolution
opt.Timeout = defaultTimeout
opt.DistanceFunc = defaultDistanceFunc
opt.PlannerConstructor = defaultPlanner
return opt
}
// plannerOptions are a set of options to be passed to a planner which will specify how to solve a motion planning problem.
type plannerOptions struct {
constraintHandler
metric Metric // Distance function to the goal
pathDist Metric // Distance function to the nearest valid point
extra map[string]interface{}
// For the below values, if left uninitialized, default values will be used. To disable, set < 0
// Max number of ik solutions to consider
MaxSolutions int `json:"max_ik_solutions"`
// Movements that score below this amount are considered "good enough" and returned immediately
MinScore float64 `json:"min_ik_score"`
// Check constraints are still met every this many mm/degrees of movement.
Resolution float64 `json:"resolution"`
// Percentage interval of max iterations after which to print debug logs
LoggingInterval float64 `json:"logging_interval"`
// Number of seconds before terminating planner
Timeout float64 `json:"timeout"`
// Function to use to measure distance between two inputs
// TODO(rb): this should really become a Metric once we change the way the constraint system works, its awkward to return 2 values here
DistanceFunc Constraint
PlannerConstructor seededPlannerConstructor
Fallback *plannerOptions
}
// SetMetric sets the distance metric for the solver.
func (p *plannerOptions) SetMetric(m Metric) {
p.metric = m
}
// SetPathDist sets the distance metric for the solver to move a constraint-violating point into a valid manifold.
func (p *plannerOptions) SetPathDist(m Metric) {
p.pathDist = m
}
// SetMaxSolutions sets the maximum number of IK solutions to generate for the planner.
func (p *plannerOptions) SetMaxSolutions(maxSolutions int) {
p.MaxSolutions = maxSolutions
}
// SetMinScore specifies the IK stopping score for the planner.
func (p *plannerOptions) SetMinScore(minScore float64) {
p.MinScore = minScore
}