-
Notifications
You must be signed in to change notification settings - Fork 110
/
ptgFrame.go
158 lines (133 loc) · 4.08 KB
/
ptgFrame.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
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
package referenceframe
import (
"fmt"
"math"
pb "go.viam.com/api/component/arm/v1"
"go.viam.com/rdk/motionplan/tpspace"
"go.viam.com/rdk/spatialmath"
)
const (
defaultSimDistMM = 2000.
defaultAlphaCnt uint = 121
)
const (
ptgIndex int = iota
trajectoryIndexWithinPTG
distanceAlongTrajectoryIndex
)
type ptgFactory func(float64, float64, float64) tpspace.PrecomputePTG
var defaultPTGs = []ptgFactory{
tpspace.NewCirclePTG,
tpspace.NewCCPTG,
tpspace.NewCCSPTG,
tpspace.NewCSPTG,
tpspace.NewAlphaPTG,
}
type ptgGridSimFrame struct {
name string
limits []Limit
geometries []spatialmath.Geometry
ptgs []tpspace.PTG
}
// NewPTGFrameFromTurningRadius will create a new Frame which is also a tpspace.PTGProvider. It will precompute the default set of
// trajectories out to a given distance, or a default distance if the given distance is <= 0.
func NewPTGFrameFromTurningRadius(name string, velocityMMps, turnRadMeters, simDist float64, geoms []spatialmath.Geometry) (Frame, error) {
if velocityMMps <= 0 {
return nil, fmt.Errorf("cannot create ptg frame, movement velocity %f must be >0", velocityMMps)
}
if turnRadMeters <= 0 {
return nil, fmt.Errorf("cannot create ptg frame, turning radius %f must be >0", turnRadMeters)
}
if simDist <= 0 {
simDist = defaultSimDistMM
}
// Get max angular velocity in radians per second
maxRPS := velocityMMps / (1000. * turnRadMeters)
pf := &ptgGridSimFrame{name: name}
err := pf.initPTGs(velocityMMps, maxRPS, simDist)
if err != nil {
return nil, err
}
pf.geometries = geoms
pf.limits = []Limit{
{Min: 0, Max: float64(len(pf.ptgs) - 1)},
{Min: 0, Max: float64(defaultAlphaCnt)},
{Min: 0, Max: simDist},
}
return pf, nil
}
func (pf *ptgGridSimFrame) DoF() []Limit {
return pf.limits
}
func (pf *ptgGridSimFrame) Name() string {
return pf.name
}
// TODO: Define some sort of config struct for a PTG frame.
func (pf *ptgGridSimFrame) MarshalJSON() ([]byte, error) {
return nil, nil
}
// Inputs are: [0] index of PTG to use, [1] index of the trajectory within that PTG, and [2] distance to travel along that trajectory.
func (pf *ptgGridSimFrame) Transform(inputs []Input) (spatialmath.Pose, error) {
ptgIdx := int(math.Round(inputs[ptgIndex].Value))
trajIdx := uint(math.Round(inputs[trajectoryIndexWithinPTG].Value))
traj := pf.ptgs[ptgIdx].Trajectory(trajIdx)
lastPose := spatialmath.NewZeroPose()
for _, trajNode := range traj {
// Walk the trajectory until we pass the specified distance
if trajNode.Dist > inputs[distanceAlongTrajectoryIndex].Value {
break
}
lastPose = trajNode.Pose
}
return lastPose, nil
}
func (pf *ptgGridSimFrame) InputFromProtobuf(jp *pb.JointPositions) []Input {
n := make([]Input, len(jp.Values))
for idx, d := range jp.Values {
n[idx] = Input{d}
}
return n
}
func (pf *ptgGridSimFrame) ProtobufFromInput(input []Input) *pb.JointPositions {
n := make([]float64, len(input))
for idx, a := range input {
n[idx] = a.Value
}
return &pb.JointPositions{Values: n}
}
func (pf *ptgGridSimFrame) Geometries(inputs []Input) (*GeometriesInFrame, error) {
if len(pf.geometries) == 0 {
return NewGeometriesInFrame(pf.Name(), nil), nil
}
transformedPose, err := pf.Transform(inputs)
if err != nil {
return nil, err
}
geoms := make([]spatialmath.Geometry, 0, len(pf.geometries))
for _, geom := range pf.geometries {
geoms = append(geoms, geom.Transform(transformedPose))
}
return NewGeometriesInFrame(pf.name, geoms), nil
}
func (pf *ptgGridSimFrame) PTGs() []tpspace.PTG {
return pf.ptgs
}
func (pf *ptgGridSimFrame) initPTGs(maxMps, maxRPS, simDist float64) error {
ptgs := []tpspace.PTG{}
for _, ptg := range defaultPTGs {
for _, k := range []float64{1., -1.} {
// Positive K calculates trajectories forwards, negative k calculates trajectories backwards
ptgGen := ptg(maxMps, maxRPS, k)
if ptgGen != nil {
// irreversible trajectories, e.g. alpha, will return nil for negative k
newptg, err := tpspace.NewPTGGridSim(ptgGen, defaultAlphaCnt, simDist)
if err != nil {
return err
}
ptgs = append(ptgs, newptg)
}
}
}
pf.ptgs = ptgs
return nil
}