-
Notifications
You must be signed in to change notification settings - Fork 110
/
combinedInverseKinematics.go
138 lines (121 loc) · 3.36 KB
/
combinedInverseKinematics.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
137
138
package motionplan
import (
"context"
"sync"
"github.com/edaniels/golog"
"go.uber.org/multierr"
"go.viam.com/utils"
"go.viam.com/rdk/referenceframe"
"go.viam.com/rdk/spatialmath"
)
// CombinedIK defines the fields necessary to run a combined solver.
type CombinedIK struct {
solvers []InverseKinematics
model referenceframe.Frame
logger golog.Logger
}
// 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
// and the first valid found solution will be returned.
func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCPU int) (*CombinedIK, error) {
ik := &CombinedIK{}
ik.model = model
if nCPU == 0 {
nCPU = 1
}
for i := 1; i <= nCPU; i++ {
nlopt, err := CreateNloptIKSolver(model, logger, -1)
nlopt.id = i
if err != nil {
return nil, err
}
ik.solvers = append(ik.solvers, nlopt)
}
ik.logger = logger
return ik, nil
}
func runSolver(ctx context.Context,
solver InverseKinematics,
c chan<- []referenceframe.Input,
pos spatialmath.Pose,
seed []referenceframe.Input,
m Metric,
rseed int,
) error {
return solver.Solve(ctx, c, pos, seed, m, rseed)
}
// 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,
c chan<- []referenceframe.Input,
newGoal spatialmath.Pose,
seed []referenceframe.Input,
m Metric,
rseed int,
) error {
ik.logger.Debugf("starting inputs: %v", seed)
startPos, err := ik.model.Transform(seed)
if err != nil {
return err
}
// This will adjust the goal position to make movements more intuitive when using incrementation near poles
ik.logger.Debugf("starting pose: %v", spatialmath.PoseToProtobuf(startPos))
ik.logger.Debugf("goal pose: %v", spatialmath.PoseToProtobuf(newGoal))
ctxWithCancel, cancel := context.WithCancel(ctx)
defer cancel()
errChan := make(chan error, len(ik.solvers))
var activeSolvers sync.WaitGroup
defer activeSolvers.Wait()
activeSolvers.Add(len(ik.solvers))
for _, solver := range ik.solvers {
rseed += 1500
parseed := rseed
thisSolver := solver
utils.PanicCapturingGo(func() {
defer activeSolvers.Done()
errChan <- runSolver(ctxWithCancel, thisSolver, c, newGoal, seed, m, parseed)
})
}
returned := 0
done := false
var collectedErrs error
// Wait until either 1) we have a success or 2) all solvers have returned false
// Multiple selects are necessary in the case where we get a ctx.Done() while there is also an error waiting
for !done {
select {
case <-ctx.Done():
return ctx.Err()
default:
}
select {
case err = <-errChan:
returned++
if err != nil {
collectedErrs = multierr.Combine(collectedErrs, err)
}
default:
if returned == len(ik.solvers) {
done = true
}
}
}
cancel()
for returned < len(ik.solvers) {
// Collect return errors from all solvers
select {
case <-ctx.Done():
return ctx.Err()
default:
}
err = <-errChan
returned++
if err != nil {
collectedErrs = multierr.Combine(collectedErrs, err)
}
}
return collectedErrs
}
// Frame returns the associated referenceframe.
func (ik *CombinedIK) Frame() referenceframe.Frame {
return ik.model
}