Skip to content
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

RSDK-2526 unable to move multiaxis gantry #2168

Merged
merged 8 commits into from
Apr 6, 2023
Merged

RSDK-2526 unable to move multiaxis gantry #2168

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
0cc6a4d
Fix indexing error
randhid Apr 5, 2023
df9a406
Add simple test
randhid Apr 5, 2023
03873c9
checkotu lcoal branch
randhid Apr 5, 2023
b90c89b
Merge
randhid Apr 5, 2023
638218e
Lint
randhid Apr 5, 2023
bbbec6c
Add comments to test
randhid Apr 6, 2023
caf86f2
Fix failign test
randhid Apr 6, 2023
da101d3
Fix second test
randhid Apr 6, 2023
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
49 changes: 21 additions & 28 deletions components/gantry/multiaxis/multiaxis.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@ type multiAxis struct {
logger golog.Logger
model referenceframe.Model
opMgr operation.SingleOperationManager
workers sync.WaitGroup
}

// Validate ensures all parts of the config are valid.
Expand Down Expand Up @@ -110,18 +111,27 @@ func (g *multiAxis) MoveToPosition(ctx context.Context, positions []float64, ext
return errors.Errorf("need position inputs for %v-axis gantry, have %v positions", len(g.subAxes), len(positions))
}

if len(positions) != len(g.lengthsMm) {
return errors.Errorf(
"number of input positions %v does not match total gantry axes count %v",
len(positions), len(g.lengthsMm),
)
}

idx := 0
for _, subAx := range g.subAxes {
subAxNum, err := subAx.Lengths(ctx, extra)
if err != nil {
return err
}

err = subAx.MoveToPosition(ctx, positions[idx:idx+len(subAxNum)-1], extra)
if err != nil {
pos := positions[idx : idx+len(subAxNum)]
idx += len(subAxNum)

err = subAx.MoveToPosition(ctx, pos, extra)
if err != nil && !errors.Is(err, context.Canceled) {
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@spieswl @nfranczak @raybjork if you'd like an update. Also, made CurrentInputs and GoToInputs fallthrough functions rather than copy-pastes of the logic to MoveToPosition and Position

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK, that matches our patterns in arm so I like the consistency.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is actually opposite from what we have in arm, where GoToInputs is the function that the basic function that is called by MoveToPosition. Can you switch these to be consistent?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Well, it's actually more consistent here, the API names are the same, but gantry.MoveToPosition is actually a functional analog to arm.MoveToJointPositions and base.MoveStraight/base.Spin. So this won't change.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@raybjork pinging you for a response.

return err
}
idx += len(subAxNum) - 1
}
return nil
}
Expand All @@ -134,20 +144,7 @@ func (g *multiAxis) GoToInputs(ctx context.Context, goal []referenceframe.Input)
ctx, done := g.opMgr.New(ctx)
defer done()

idx := 0
for _, subAx := range g.subAxes {
subAxNum, err := subAx.Lengths(ctx, nil)
if err != nil {
return err
}

err = subAx.MoveToPosition(ctx, referenceframe.InputsToFloats(goal[idx:idx+len(subAxNum)-1]), nil)
if err != nil {
return err
}
idx += len(subAxNum) - 1
}
return nil
return g.MoveToPosition(ctx, referenceframe.InputsToFloats(goal), nil)
}

// Position returns the position in millimeters.
Expand Down Expand Up @@ -180,15 +177,14 @@ func (g *multiAxis) Lengths(ctx context.Context, extra map[string]interface{}) (
func (g *multiAxis) Stop(ctx context.Context, extra map[string]interface{}) error {
ctx, done := g.opMgr.New(ctx)
defer done()
wg := sync.WaitGroup{}
for _, subAx := range g.subAxes {
currG := subAx
wg.Add(1)
g.workers.Add(1)
utils.ManagedGo(func() {
if err := currG.Stop(ctx, extra); err != nil {
g.logger.Errorw("failed to stop subaxis", "error", err)
}
}, wg.Done)
}, g.workers.Done)
}
return nil
}
Expand All @@ -203,15 +199,12 @@ func (g *multiAxis) CurrentInputs(ctx context.Context) ([]referenceframe.Input,
if len(g.subAxes) == 0 {
return nil, errors.New("no subaxes found for inputs")
}
inputs := []float64{}
for _, subAx := range g.subAxes {
in, err := subAx.Position(ctx, nil)
if err != nil {
return nil, err
}
inputs = append(inputs, in...)
positions, err := g.Position(ctx, nil)
if err != nil {
return nil, err
}
return referenceframe.FloatsToInputs(inputs), nil

return referenceframe.FloatsToInputs(positions), nil
}

// ModelFrame returns the frame model of the Gantry.
Expand Down
123 changes: 119 additions & 4 deletions components/gantry/multiaxis/multiaxis_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,12 +116,12 @@ func TestMoveToPosition(t *testing.T) {
err := fakemultiaxis.MoveToPosition(ctx, positions, nil)
test.That(t, err, test.ShouldNotBeNil)

fakemultiaxis = &multiAxis{subAxes: threeAxes}
fakemultiaxis = &multiAxis{subAxes: threeAxes, lengthsMm: []float64{1, 2, 3}}
positions = []float64{1, 2, 3}
err = fakemultiaxis.MoveToPosition(ctx, positions, nil)
test.That(t, err, test.ShouldBeNil)

fakemultiaxis = &multiAxis{subAxes: twoAxes}
fakemultiaxis = &multiAxis{subAxes: twoAxes, lengthsMm: []float64{1, 2}}
positions = []float64{1, 2}
err = fakemultiaxis.MoveToPosition(ctx, positions, nil)
test.That(t, err, test.ShouldBeNil)
Expand All @@ -135,12 +135,12 @@ func TestGoToInputs(t *testing.T) {
err := fakemultiaxis.GoToInputs(ctx, inputs)
test.That(t, err, test.ShouldNotBeNil)

fakemultiaxis = &multiAxis{subAxes: threeAxes}
fakemultiaxis = &multiAxis{subAxes: threeAxes, lengthsMm: []float64{1, 2, 3}}
inputs = []referenceframe.Input{{Value: 1}, {Value: 2}, {Value: 3}}
err = fakemultiaxis.GoToInputs(ctx, inputs)
test.That(t, err, test.ShouldBeNil)

fakemultiaxis = &multiAxis{subAxes: twoAxes}
fakemultiaxis = &multiAxis{subAxes: twoAxes, lengthsMm: []float64{1, 2}}
inputs = []referenceframe.Input{{Value: 1}, {Value: 2}}
err = fakemultiaxis.GoToInputs(ctx, inputs)
test.That(t, err, test.ShouldBeNil)
Expand Down Expand Up @@ -218,3 +218,118 @@ func TestModelFrame(t *testing.T) {
model = fakemultiaxis.ModelFrame()
test.That(t, model, test.ShouldNotBeNil)
}

func createComplexDeps() registry.Dependencies {
position1 := []float64{6, 5}
mAx1 := &inject.Gantry{
PositionFunc: func(ctx context.Context, extra map[string]interface{}) ([]float64, error) {
return position1, nil
},
MoveToPositionFunc: func(ctx context.Context, pos []float64, extra map[string]interface{}) error {
if move, _ := extra["move"].(bool); move {
position1[0] += pos[0]
position1[1] += pos[1]
}

return nil
},
LengthsFunc: func(ctx context.Context, extra map[string]interface{}) ([]float64, error) {
return []float64{100, 101}, nil
},
StopFunc: func(ctx context.Context, extra map[string]interface{}) error {
return nil
},
}

position2 := []float64{9, 8, 7}
mAx2 := &inject.Gantry{
PositionFunc: func(ctx context.Context, extra map[string]interface{}) ([]float64, error) {
return position2, nil
},
MoveToPositionFunc: func(ctx context.Context, pos []float64, extra map[string]interface{}) error {
if move, _ := extra["move"].(bool); move {
position2[0] += pos[0]
position2[1] += pos[1]
position2[2] += pos[2]
}
return nil
},
LengthsFunc: func(ctx context.Context, extra map[string]interface{}) ([]float64, error) {
return []float64{102, 103, 104}, nil
},
StopFunc: func(ctx context.Context, extra map[string]interface{}) error {
return nil
},
}

fakeMotor := &fm.Motor{}

deps := make(registry.Dependencies)
deps[gantry.Named("1")] = mAx1
deps[gantry.Named("2")] = mAx2
deps[motor.Named(fakeMotor.Name)] = fakeMotor
return deps
}

func TestComplexMultiAxis(t *testing.T) {
ctx := context.Background()
logger := golog.NewTestLogger(t)
cfg := config.Component{
Name: "complexGantry",
ConvertedAttributes: &AttrConfig{
SubAxes: []string{"1", "2"},
},
}
deps := createComplexDeps()

g, err := newMultiAxis(ctx, deps, cfg, logger)
test.That(t, err, test.ShouldBeNil)

t.Run("too many inputs", func(t *testing.T) {
err = g.MoveToPosition(ctx, []float64{1, 2, 3, 4, 5, 6}, nil)
test.That(t, err, test.ShouldNotBeNil)
})

t.Run("too few inputs", func(t *testing.T) {
err = g.MoveToPosition(ctx, []float64{1, 2, 3, 4}, nil)
test.That(t, err, test.ShouldNotBeNil)
})

t.Run("just right inputs", func(t *testing.T) {
pos, err := g.Position(ctx, nil)
test.That(t, err, test.ShouldBeNil)
test.That(t, pos, test.ShouldResemble, []float64{6, 5, 9, 8, 7})
})

t.Run(
"test that multiaxis moves and each subaxes moves correctly",
func(t *testing.T) {
extra := map[string]interface{}{"move": true}
err = g.MoveToPosition(ctx, []float64{1, 2, 3, 4, 5}, extra)
test.That(t, err, test.ShouldBeNil)

pos, err := g.Position(ctx, nil)
test.That(t, err, test.ShouldBeNil)
test.That(t, pos, test.ShouldNotResemble, []float64{6, 5, 9, 8, 7})

// This section tests out that each subaxes has moved, and moved the correct amount
// according to it's input lengths
currG, ok := g.(*multiAxis)
test.That(t, ok, test.ShouldBeTrue)

// This loop mimics the loop in MoveToposition to check that the correct
// positions are sent to each subaxis
idx := 0
for _, subAx := range currG.subAxes {
lengths, err := subAx.Lengths(ctx, nil)
test.That(t, err, test.ShouldBeNil)

subAxPos, err := subAx.Position(ctx, nil)
test.That(t, err, test.ShouldBeNil)
test.That(t, len(subAxPos), test.ShouldEqual, len(lengths))

test.That(t, subAxPos, test.ShouldResemble, pos[idx:idx+len(lengths)])
idx += len(lengths)
}
})
}
7 changes: 4 additions & 3 deletions components/gantry/oneaxis/oneaxis.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -265,7 +265,7 @@ func (g *oneAxis) homeTwoLimSwitch(ctx context.Context) error {
g.positionLimits = []float64{positionA, positionB}

// Go backwards so limit stops are not hit.
x := g.rotationalToLinear(0.8 * g.lengthMm)
x := g.linearToRotational(0.8 * g.lengthMm)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Oof, this looks like a frustrating bug to find! Nice work here.

err = g.motor.GoTo(ctx, g.rpm, x, nil)
if err != nil {
return err
Expand Down Expand Up @@ -302,7 +302,7 @@ func (g *oneAxis) homeEncoder(ctx context.Context) error {
return nil
}

func (g *oneAxis) rotationalToLinear(positions float64) float64 {
func (g *oneAxis) linearToRotational(positions float64) float64 {
theRange := g.positionLimits[1] - g.positionLimits[0]
x := positions / g.lengthMm
x = g.positionLimits[0] + (x * theRange)
Expand Down Expand Up @@ -397,7 +397,7 @@ func (g *oneAxis) MoveToPosition(ctx context.Context, positions []float64, extra
return fmt.Errorf("oneAxis %s out of range (%.2f) min: 0 max: %.2f", g.name, positions[0], g.lengthMm)
}

x := g.rotationalToLinear(positions[0])
x := g.linearToRotational(positions[0])
// Limit switch errors that stop the motors.
// Currently needs to be moved by underlying gantry motor.
if len(g.limitSwitchPins) > 0 {
Expand Down Expand Up @@ -434,6 +434,7 @@ func (g *oneAxis) MoveToPosition(ctx context.Context, positions []float64, extra
}
}

g.logger.Debugf("gantry (%s) going to %.2f at speed %.2f", g.name, x, g.rpm)
err := g.motor.GoTo(ctx, g.rpm, x, extra)
if err != nil {
return err
Expand Down