Skip to content

Commit cff8905

Browse files
raybjorkraybjork
authored
RSDK-9645 introduce consistent naming convention for frame system map objects (viamrobotics#4664)
1 parent a8263b2 commit cff8905

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

46 files changed

+415
-396
lines changed

‎components/base/kinematicbase/execution.go

+1-1
Original file line numberDiff line numberDiff line change
@@ -597,7 +597,7 @@ func (ptgk *ptgBaseKinematics) stepsToPlan(steps []arcStep, parentFrame string)
597597
traj := motionplan.Trajectory{}
598598
path := motionplan.Path{}
599599
for _, step := range steps {
600-
traj = append(traj, map[string][]referenceframe.Input{ptgk.Kinematics().Name(): step.arcSegment.EndConfiguration})
600+
traj = append(traj, referenceframe.FrameSystemInputs{ptgk.Kinematics().Name(): step.arcSegment.EndConfiguration})
601601
path = append(path, map[string]*referenceframe.PoseInFrame{
602602
ptgk.Kinematics().Name(): referenceframe.NewPoseInFrame(parentFrame, step.arcSegment.EndPosition),
603603
})

‎components/base/kinematicbase/ptgKinematics.go

+1-1
Original file line numberDiff line numberDiff line change
@@ -206,7 +206,7 @@ func (ptgk *ptgBaseKinematics) ExecutionState(ctx context.Context) (motionplan.E
206206
return motionplan.NewExecutionState(
207207
currentPlan,
208208
currentIdx,
209-
map[string][]referenceframe.Input{ptgk.Kinematics().Name(): currentInputs},
209+
referenceframe.FrameSystemInputs{ptgk.Kinematics().Name(): currentInputs},
210210
map[string]*referenceframe.PoseInFrame{ptgk.LocalizationFrame().Name(): actualPIF},
211211
)
212212
}

‎components/base/kinematicbase/ptgKinematics_test.go

+12-6
Original file line numberDiff line numberDiff line change
@@ -75,10 +75,13 @@ func TestPTGKinematicsNoGeom(t *testing.T) {
7575

7676
test.That(t, err, test.ShouldBeNil)
7777
fs.AddFrame(f, fs.World())
78-
inputMap := referenceframe.StartPositions(fs)
78+
inputMap := referenceframe.NewZeroInputs(fs)
7979

80-
startState := motionplan.NewPlanState(motionplan.PathState{f.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)}, inputMap)
81-
goalState := motionplan.NewPlanState(motionplan.PathState{f.Name(): dstPIF}, nil)
80+
startState := motionplan.NewPlanState(
81+
referenceframe.FrameSystemPoses{f.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)},
82+
inputMap,
83+
)
84+
goalState := motionplan.NewPlanState(referenceframe.FrameSystemPoses{f.Name(): dstPIF}, nil)
8285

8386
plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{
8487
Logger: logger,
@@ -153,7 +156,7 @@ func TestPTGKinematicsWithGeom(t *testing.T) {
153156
f := kb.Kinematics()
154157
test.That(t, err, test.ShouldBeNil)
155158
fs.AddFrame(f, fs.World())
156-
inputMap := referenceframe.StartPositions(fs)
159+
inputMap := referenceframe.NewZeroInputs(fs)
157160

158161
obstacle, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2000, 0, 0}), r3.Vector{1, 1, 1}, "")
159162
test.That(t, err, test.ShouldBeNil)
@@ -165,8 +168,11 @@ func TestPTGKinematicsWithGeom(t *testing.T) {
165168
)
166169
test.That(t, err, test.ShouldBeNil)
167170

168-
startState := motionplan.NewPlanState(motionplan.PathState{f.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)}, inputMap)
169-
goalState := motionplan.NewPlanState(motionplan.PathState{f.Name(): dstPIF}, nil)
171+
startState := motionplan.NewPlanState(
172+
referenceframe.FrameSystemPoses{f.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)},
173+
inputMap,
174+
)
175+
goalState := motionplan.NewPlanState(referenceframe.FrameSystemPoses{f.Name(): dstPIF}, nil)
170176
plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{
171177
Logger: logger,
172178
Goals: []*motionplan.PlanState{goalState},

‎motionplan/cBiRRT.go

+3-3
Original file line numberDiff line numberDiff line change
@@ -134,7 +134,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
134134
defer cancel()
135135
mp.start = time.Now()
136136

137-
var seed map[string][]referenceframe.Input
137+
var seed referenceframe.FrameSystemInputs
138138
// Pick a random (first in map) seed node to create the first interp node
139139
for sNode, parent := range rrt.maps.startMap {
140140
if parent == nil {
@@ -351,8 +351,8 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
351351
ctx context.Context,
352352
randseed *rand.Rand,
353353
seedInputs,
354-
target map[string][]referenceframe.Input,
355-
) map[string][]referenceframe.Input {
354+
target referenceframe.FrameSystemInputs,
355+
) referenceframe.FrameSystemInputs {
356356
for i := 0; i < maxNearIter; i++ {
357357
select {
358358
case <-ctx.Done():

‎motionplan/cBiRRT_test.go

+4-4
Original file line numberDiff line numberDiff line change
@@ -39,19 +39,19 @@ func TestSimpleLinearMotion(t *testing.T) {
3939
goalPos := spatialmath.NewPose(r3.Vector{X: 206, Y: 100, Z: 120.5}, &spatialmath.OrientationVectorDegrees{OY: -1})
4040

4141
opt := newBasicPlannerOptions()
42-
goalMetric := opt.getGoalMetric(PathState{m.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos)})
42+
goalMetric := opt.getGoalMetric(referenceframe.FrameSystemPoses{m.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos)})
4343
fs := referenceframe.NewEmptyFrameSystem("")
4444
fs.AddFrame(m, fs.World())
4545
mp, err := newCBiRRTMotionPlanner(fs, rand.New(rand.NewSource(42)), logger, opt)
4646
test.That(t, err, test.ShouldBeNil)
4747
cbirrt, _ := mp.(*cBiRRTMotionPlanner)
48-
solutions, err := mp.getSolutions(ctx, map[string][]referenceframe.Input{m.Name(): home7}, goalMetric)
48+
solutions, err := mp.getSolutions(ctx, referenceframe.FrameSystemInputs{m.Name(): home7}, goalMetric)
4949
test.That(t, err, test.ShouldBeNil)
5050

51-
near1 := &basicNode{q: map[string][]referenceframe.Input{m.Name(): home7}}
51+
near1 := &basicNode{q: referenceframe.FrameSystemInputs{m.Name(): home7}}
5252
seedMap := make(map[node]node)
5353
seedMap[near1] = nil
54-
target := map[string][]referenceframe.Input{m.Name(): interp}
54+
target := referenceframe.FrameSystemInputs{m.Name(): interp}
5555

5656
goalMap := make(map[node]node)
5757

‎motionplan/check.go

+2-2
Original file line numberDiff line numberDiff line change
@@ -77,7 +77,7 @@ func checkPlanRelative(
7777
sfPlanner *planManager,
7878
) error {
7979
var err error
80-
toWorld := func(pif *referenceframe.PoseInFrame, inputs map[string][]referenceframe.Input) (*referenceframe.PoseInFrame, error) {
80+
toWorld := func(pif *referenceframe.PoseInFrame, inputs referenceframe.FrameSystemInputs) (*referenceframe.PoseInFrame, error) {
8181
transformable, err := fs.Transform(inputs, pif, referenceframe.World)
8282
if err != nil {
8383
return nil, err
@@ -331,7 +331,7 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDist
331331
}
332332
for _, interpConfig := range interpolatedConfigurations {
333333
poseInPathTf, err := sfPlanner.fs.Transform(
334-
map[string][]referenceframe.Input{checkFrame.Name(): interpConfig},
334+
referenceframe.FrameSystemInputs{checkFrame.Name(): interpConfig},
335335
referenceframe.NewZeroPoseInFrame(checkFrame.Name()),
336336
parent.Name(),
337337
)

‎motionplan/collision_test.go

+7-7
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@ import (
66
"github.com/golang/geo/r3"
77
"go.viam.com/test"
88

9-
frame "go.viam.com/rdk/referenceframe"
9+
"go.viam.com/rdk/referenceframe"
1010
spatial "go.viam.com/rdk/spatialmath"
1111
"go.viam.com/rdk/utils"
1212
)
@@ -73,21 +73,21 @@ func TestCheckCollisions(t *testing.T) {
7373

7474
// case 2: zero position of xArm6 arm - should have number of collisions = to number of geometries - 1
7575
// no external geometries considered, self collision only
76-
m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "")
76+
m, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "")
7777
test.That(t, err, test.ShouldBeNil)
78-
gf, _ := m.Geometries(make([]frame.Input, len(m.DoF())))
78+
gf, _ := m.Geometries(make([]referenceframe.Input, len(m.DoF())))
7979
test.That(t, gf, test.ShouldNotBeNil)
8080
cg, err = newCollisionGraph(gf.Geometries(), gf.Geometries(), nil, true, defaultCollisionBufferMM)
8181
test.That(t, err, test.ShouldBeNil)
8282
test.That(t, len(cg.collisions(defaultCollisionBufferMM)), test.ShouldEqual, 4)
8383
}
8484

8585
func TestUniqueCollisions(t *testing.T) {
86-
m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "")
86+
m, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/example_kinematics/xarm6_kinematics_test.json"), "")
8787
test.That(t, err, test.ShouldBeNil)
8888

8989
// zero position of xarm6 arm
90-
input := make([]frame.Input, len(m.DoF()))
90+
input := make([]referenceframe.Input, len(m.DoF()))
9191
internalGeometries, _ := m.Geometries(input)
9292
test.That(t, internalGeometries, test.ShouldNotBeNil)
9393
zeroPositionCG, err := newCollisionGraph(
@@ -100,7 +100,7 @@ func TestUniqueCollisions(t *testing.T) {
100100
test.That(t, err, test.ShouldBeNil)
101101

102102
// case 1: no self collision - check no new collisions are returned
103-
input[0] = frame.Input{Value: 1}
103+
input[0] = referenceframe.Input{Value: 1}
104104
internalGeometries, _ = m.Geometries(input)
105105
test.That(t, internalGeometries, test.ShouldNotBeNil)
106106
cg, err := newCollisionGraph(
@@ -114,7 +114,7 @@ func TestUniqueCollisions(t *testing.T) {
114114
test.That(t, len(cg.collisions(defaultCollisionBufferMM)), test.ShouldEqual, 0)
115115

116116
// case 2: self collision - check only new collisions are returned
117-
input[4] = frame.Input{Value: 2}
117+
input[4] = referenceframe.Input{Value: 2}
118118
internalGeometries, _ = m.Geometries(input)
119119
test.That(t, internalGeometries, test.ShouldNotBeNil)
120120
cg, err = newCollisionGraph(

‎motionplan/constraint.go

+13-13
Original file line numberDiff line numberDiff line change
@@ -709,7 +709,7 @@ func (c *Constraints) GetCollisionSpecification() []CollisionSpecification {
709709
type fsPathConstraint struct {
710710
metricMap map[string]ik.StateMetric
711711
constraintMap map[string]StateConstraint
712-
goalMap PathState
712+
goalMap referenceframe.FrameSystemPoses
713713
fs referenceframe.FrameSystem
714714
}
715715

@@ -754,8 +754,8 @@ func (fpc *fsPathConstraint) metric(state *ik.StateFS) float64 {
754754

755755
func newFsPathConstraintSeparatedLinOrientTol(
756756
fs referenceframe.FrameSystem,
757-
startCfg map[string][]referenceframe.Input,
758-
from, to PathState,
757+
startCfg referenceframe.FrameSystemInputs,
758+
from, to referenceframe.FrameSystemPoses,
759759
constructor func(spatial.Pose, spatial.Pose, float64, float64) (StateConstraint, ik.StateMetric),
760760
linTol, orientTol float64,
761761
) (*fsPathConstraint, error) {
@@ -786,8 +786,8 @@ func newFsPathConstraintSeparatedLinOrientTol(
786786

787787
func newFsPathConstraintTol(
788788
fs referenceframe.FrameSystem,
789-
startCfg map[string][]referenceframe.Input,
790-
from, to PathState,
789+
startCfg referenceframe.FrameSystemInputs,
790+
from, to referenceframe.FrameSystemPoses,
791791
constructor func(spatial.Pose, spatial.Pose, float64) (StateConstraint, ik.StateMetric),
792792
tolerance float64,
793793
) (*fsPathConstraint, error) {
@@ -821,8 +821,8 @@ func newFsPathConstraintTol(
821821
// their respective orientations, as well as a metric which returns the distance to that valid region.
822822
func CreateSlerpOrientationConstraintFS(
823823
fs referenceframe.FrameSystem,
824-
startCfg map[string][]referenceframe.Input,
825-
from, to PathState,
824+
startCfg referenceframe.FrameSystemInputs,
825+
from, to referenceframe.FrameSystemPoses,
826826
tolerance float64,
827827
) (StateFSConstraint, ik.StateFSMetric, error) {
828828
constraintInternal, err := newFsPathConstraintTol(fs, startCfg, from, to, NewSlerpOrientationConstraint, tolerance)
@@ -837,8 +837,8 @@ func CreateSlerpOrientationConstraintFS(
837837
// line segment between their respective positions, as well as a metric which returns the distance to that valid region.
838838
func CreateLineConstraintFS(
839839
fs referenceframe.FrameSystem,
840-
startCfg map[string][]referenceframe.Input,
841-
from, to PathState,
840+
startCfg referenceframe.FrameSystemInputs,
841+
from, to referenceframe.FrameSystemPoses,
842842
tolerance float64,
843843
) (StateFSConstraint, ik.StateFSMetric, error) {
844844
// Need to define a constructor here since NewLineConstraint takes r3.Vectors, not poses
@@ -857,8 +857,8 @@ func CreateLineConstraintFS(
857857
// orientation deviation measured by norm of the R3AA orientation difference to the slerp path between start/goal orientations.
858858
func CreateAbsoluteLinearInterpolatingConstraintFS(
859859
fs referenceframe.FrameSystem,
860-
startCfg map[string][]referenceframe.Input,
861-
from, to PathState,
860+
startCfg referenceframe.FrameSystemInputs,
861+
from, to referenceframe.FrameSystemPoses,
862862
linTol, orientTol float64,
863863
) (StateFSConstraint, ik.StateFSMetric, error) {
864864
constraintInternal, err := newFsPathConstraintSeparatedLinOrientTol(
@@ -881,8 +881,8 @@ func CreateAbsoluteLinearInterpolatingConstraintFS(
881881
// from start to goal.
882882
func CreateProportionalLinearInterpolatingConstraintFS(
883883
fs referenceframe.FrameSystem,
884-
startCfg map[string][]referenceframe.Input,
885-
from, to PathState,
884+
startCfg referenceframe.FrameSystemInputs,
885+
from, to referenceframe.FrameSystemPoses,
886886
linTol, orientTol float64,
887887
) (StateFSConstraint, ik.StateFSMetric, error) {
888888
constraintInternal, err := newFsPathConstraintSeparatedLinOrientTol(

‎motionplan/constraint_handler.go

+6-6
Original file line numberDiff line numberDiff line change
@@ -113,7 +113,7 @@ func interpolateSegment(ci *ik.Segment, resolution float64) ([][]referenceframe.
113113
return nil, err
114114
}
115115

116-
steps := PathStateCount(ci.StartPosition, ci.EndPosition, resolution)
116+
steps := CalculateStepCount(ci.StartPosition, ci.EndPosition, resolution)
117117
if steps < defaultMinStepCount {
118118
// Minimum step count ensures we are not missing anything
119119
steps = defaultMinStepCount
@@ -133,7 +133,7 @@ func interpolateSegment(ci *ik.Segment, resolution float64) ([][]referenceframe.
133133

134134
// interpolateSegmentFS is a helper function which produces a list of intermediate inputs, between the start and end
135135
// configuration of a segment at a given resolution value.
136-
func interpolateSegmentFS(ci *ik.SegmentFS, resolution float64) ([]map[string][]referenceframe.Input, error) {
136+
func interpolateSegmentFS(ci *ik.SegmentFS, resolution float64) ([]referenceframe.FrameSystemInputs, error) {
137137
// Find the frame with the most steps by calculating steps for each frame
138138
maxSteps := defaultMinStepCount
139139
for frameName, startConfig := range ci.StartConfiguration {
@@ -163,17 +163,17 @@ func interpolateSegmentFS(ci *ik.SegmentFS, resolution float64) ([]map[string][]
163163
}
164164

165165
// Calculate steps needed for this frame
166-
steps := PathStateCount(startPos, endPos, resolution)
166+
steps := CalculateStepCount(startPos, endPos, resolution)
167167
if steps > maxSteps {
168168
maxSteps = steps
169169
}
170170
}
171171

172172
// Create interpolated configurations for all frames
173-
var interpolatedConfigurations []map[string][]referenceframe.Input
173+
var interpolatedConfigurations []referenceframe.FrameSystemInputs
174174
for i := 0; i <= maxSteps; i++ {
175175
interp := float64(i) / float64(maxSteps)
176-
frameConfigs := make(map[string][]referenceframe.Input)
176+
frameConfigs := make(referenceframe.FrameSystemInputs)
177177

178178
// Interpolate each frame's configuration
179179
for frameName, startConfig := range ci.StartConfiguration {
@@ -318,7 +318,7 @@ func (c *ConstraintHandler) CheckStateConstraintsAcrossSegmentFS(ci *ik.SegmentF
318318
if err != nil {
319319
return false, nil
320320
}
321-
var lastGood map[string][]referenceframe.Input
321+
var lastGood referenceframe.FrameSystemInputs
322322
for i, interpConfig := range interpolatedConfigurations {
323323
interpC := &ik.StateFS{FS: ci.FS, Configuration: interpConfig}
324324
pass, _ := c.CheckStateFSConstraints(interpC)

‎motionplan/constraint_test.go

+8-12
Original file line numberDiff line numberDiff line change
@@ -29,14 +29,10 @@ func TestIKTolerances(t *testing.T) {
2929
test.That(t, err, test.ShouldBeNil)
3030

3131
// Test inability to arrive at another position due to orientation
32-
goal := &PlanState{poses: PathState{m.Name(): frame.NewPoseInFrame(frame.World, spatial.NewPoseFromProtobuf(&commonpb.Pose{
33-
X: -46,
34-
Y: 0,
35-
Z: 372,
36-
OX: -1.78,
37-
OY: -3.3,
38-
OZ: -1.11,
39-
}))}}
32+
goal := &PlanState{poses: frame.FrameSystemPoses{m.Name(): frame.NewPoseInFrame(
33+
frame.World,
34+
spatial.NewPoseFromProtobuf(&commonpb.Pose{X: -46, Y: 0, Z: 372, OX: -1.78, OY: -3.3, OZ: -1.11}),
35+
)}}
4036
seed := &PlanState{configuration: map[string][]frame.Input{m.Name(): frame.FloatsToInputs(make([]float64, 6))}}
4137
_, err = mp.plan(context.Background(), seed, goal)
4238
test.That(t, err, test.ShouldNotBeNil)
@@ -151,8 +147,8 @@ func TestLineFollow(t *testing.T) {
151147

152148
opt := newBasicPlannerOptions()
153149
startCfg := map[string][]frame.Input{m.Name(): m.InputFromProtobuf(mp1)}
154-
from := PathState{markerFrame.Name(): frame.NewPoseInFrame(markerFrame.Name(), p1)}
155-
to := PathState{markerFrame.Name(): frame.NewPoseInFrame(goalFrame.Name(), p2)}
150+
from := frame.FrameSystemPoses{markerFrame.Name(): frame.NewPoseInFrame(markerFrame.Name(), p1)}
151+
to := frame.FrameSystemPoses{markerFrame.Name(): frame.NewPoseInFrame(goalFrame.Name(), p2)}
156152

157153
validFunc, gradFunc, err := CreateLineConstraintFS(fs, startCfg, from, to, 0.001)
158154
test.That(t, err, test.ShouldBeNil)
@@ -220,7 +216,7 @@ func TestCollisionConstraints(t *testing.T) {
220216
fs := frame.NewEmptyFrameSystem("test")
221217
err = fs.AddFrame(model, fs.Frame(frame.World))
222218
test.That(t, err, test.ShouldBeNil)
223-
seedMap := frame.StartPositions(fs)
219+
seedMap := frame.NewZeroInputs(fs)
224220
handler := &ConstraintHandler{}
225221

226222
// create robot collision entities
@@ -284,7 +280,7 @@ func BenchmarkCollisionConstraints(b *testing.B) {
284280
fs := frame.NewEmptyFrameSystem("test")
285281
err = fs.AddFrame(model, fs.Frame(frame.World))
286282
test.That(b, err, test.ShouldBeNil)
287-
seedMap := frame.StartPositions(fs)
283+
seedMap := frame.NewZeroInputs(fs)
288284
handler := &ConstraintHandler{}
289285

290286
// create robot collision entities

‎motionplan/ik/metrics.go

+3-3
Original file line numberDiff line numberDiff line change
@@ -25,8 +25,8 @@ type Segment struct {
2525
// SegmentFS is a referenceframe.FrameSystem-specific contains all the information a constraint needs to determine validity for a movement.
2626
// It contains the starting inputs, the ending inputs, and the framesystem it refers to.
2727
type SegmentFS struct {
28-
StartConfiguration map[string][]referenceframe.Input
29-
EndConfiguration map[string][]referenceframe.Input
28+
StartConfiguration referenceframe.FrameSystemInputs
29+
EndConfiguration referenceframe.FrameSystemInputs
3030
FS referenceframe.FrameSystem
3131
}
3232

@@ -62,7 +62,7 @@ type State struct {
6262
// framesystem. It contains inputs, the corresponding poses, and the frame it refers to.
6363
// Pose field may be empty, and may be filled in by a constraint that needs it.
6464
type StateFS struct {
65-
Configuration map[string][]referenceframe.Input
65+
Configuration referenceframe.FrameSystemInputs
6666
FS referenceframe.FrameSystem
6767
}
6868

‎motionplan/kinematic_test.go

+2-2
Original file line numberDiff line numberDiff line change
@@ -163,7 +163,7 @@ func TestDynamicFrameSystemXArm(t *testing.T) {
163163
test.That(t, err, test.ShouldBeNil)
164164
fs.AddFrame(model, fs.World())
165165

166-
positions := frame.StartPositions(fs)
166+
positions := frame.NewZeroInputs(fs)
167167

168168
// World point of xArm at 0 position
169169
poseWorld1 := spatial.NewPoseFromPoint(r3.Vector{207, 0, 112})
@@ -227,7 +227,7 @@ func TestComplicatedDynamicFrameSystem(t *testing.T) {
227227
test.That(t, err, test.ShouldBeNil)
228228
fs.AddFrame(urCamera, modelUR5e)
229229

230-
positions := frame.StartPositions(fs)
230+
positions := frame.NewZeroInputs(fs)
231231

232232
poseUR5e := spatial.NewPoseFromPoint(r3.Vector{-717.2, -132.9, 262.8})
233233
// Camera translates by 30, gripper is pointed at -Y

0 commit comments

Comments
 (0)