From 88a411113a389136e9de5f042a5913dabe909338 Mon Sep 17 00:00:00 2001 From: Eliot Horowitz Date: Sun, 21 May 2023 09:21:37 -0400 Subject: [PATCH] moved base to a module at https://github.com/erh/viamboatbase --- components/base/boat/base.go | 312 ----------------------- components/base/boat/base_test.go | 51 ---- components/base/boat/boat_config.go | 162 ------------ components/base/boat/boat_config_test.go | 178 ------------- components/base/boat/motor.go | 55 ---- components/base/boat/motor_test.go | 81 ------ components/base/register/register.go | 1 - 7 files changed, 840 deletions(-) delete mode 100644 components/base/boat/base.go delete mode 100644 components/base/boat/base_test.go delete mode 100644 components/base/boat/boat_config.go delete mode 100644 components/base/boat/boat_config_test.go delete mode 100644 components/base/boat/motor.go delete mode 100644 components/base/boat/motor_test.go diff --git a/components/base/boat/base.go b/components/base/boat/base.go deleted file mode 100644 index 6d3a4a185a1..00000000000 --- a/components/base/boat/base.go +++ /dev/null @@ -1,312 +0,0 @@ -//go:build !windows - -// Package boat implements a base for a boat with support for N motors in any position or angle -// This is an Experimental package -package boat - -import ( - "context" - "errors" - "math" - "sync" - "time" - - "github.com/edaniels/golog" - "github.com/golang/geo/r3" - "go.uber.org/multierr" - "go.viam.com/utils" - - "go.viam.com/rdk/components/base" - "go.viam.com/rdk/components/motor" - "go.viam.com/rdk/components/movementsensor" - "go.viam.com/rdk/operation" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/spatialmath" -) - -var model = resource.DefaultModelFamily.WithModel("boat") - -func init() { - boatComp := resource.Registration[base.Base, *boatConfig]{ - Constructor: func( - ctx context.Context, deps resource.Dependencies, conf resource.Config, logger golog.Logger, - ) (base.Base, error) { - return createBoat(deps, conf, logger) - }, - } - resource.RegisterComponent(base.API, model, boatComp) -} - -func createBoat(deps resource.Dependencies, conf resource.Config, logger golog.Logger) (base.LocalBase, error) { - newConf, err := resource.NativeConfig[*boatConfig](conf) - if err != nil { - return nil, err - } - - if newConf.WidthMM <= 0 { - return nil, errors.New("width has to be > 0") - } - - if newConf.LengthMM <= 0 { - return nil, errors.New("length has to be > 0") - } - - theBoat := &boat{ - Named: conf.ResourceName().AsNamed(), - cfg: newConf, - logger: logger, - } - - for _, mc := range newConf.Motors { - m, err := motor.FromDependencies(deps, mc.Name) - if err != nil { - return nil, err - } - theBoat.motors = append(theBoat.motors, m) - } - - if newConf.IMU != "" { - var err error - theBoat.imu, err = movementsensor.FromDependencies(deps, newConf.IMU) - if err != nil { - return nil, err - } - } - return theBoat, nil -} - -type boatState struct { - threadStarted bool - velocityControlled bool - - lastPower []float64 - lastPowerLinear, lastPowerAngular r3.Vector - velocityLinearGoal, velocityAngularGoal r3.Vector -} - -type boat struct { - resource.Named - resource.AlwaysRebuild - - cfg *boatConfig - motors []motor.Motor - imu movementsensor.MovementSensor - - opMgr operation.SingleOperationManager - - state boatState - stateMutex sync.Mutex - - cancel context.CancelFunc - waitGroup sync.WaitGroup - - logger golog.Logger -} - -func (b *boat) MoveStraight(ctx context.Context, distanceMm int, mmPerSec float64, extra map[string]interface{}) error { - if distanceMm < 0 { - mmPerSec *= -1 - distanceMm *= -1 - } - err := b.SetVelocity(ctx, r3.Vector{Y: mmPerSec}, r3.Vector{}, extra) - if err != nil { - return err - } - s := time.Duration(float64(time.Millisecond) * math.Abs(float64(distanceMm))) - utils.SelectContextOrWait(ctx, s) - return b.Stop(ctx, nil) -} - -func (b *boat) Spin(ctx context.Context, angleDeg, degsPerSec float64, extra map[string]interface{}) error { - millis := 1000 * (angleDeg / degsPerSec) - err := b.SetVelocity(ctx, r3.Vector{}, r3.Vector{Z: -1 * degsPerSec}, extra) - if err != nil { - return err - } - utils.SelectContextOrWait(ctx, time.Duration(float64(time.Millisecond)*millis)) - return b.Stop(ctx, nil) -} - -func (b *boat) startVelocityThread() error { - if b.imu == nil { - return errors.New("no imu") - } - - var ctx context.Context - ctx, b.cancel = context.WithCancel(context.Background()) - - b.waitGroup.Add(1) - go func() { - defer b.waitGroup.Done() - - for { - utils.SelectContextOrWait(ctx, time.Millisecond*500) - err := b.velocityThreadLoop(ctx) - if err != nil { - if errors.Is(err, context.Canceled) { - return - } - b.logger.Warn(err) - } - } - }() - - return nil -} - -func (b *boat) velocityThreadLoop(ctx context.Context) error { - av, err := b.imu.AngularVelocity(ctx, make(map[string]interface{})) - if err != nil { - return err - } - - b.stateMutex.Lock() - - if !b.state.velocityControlled { - b.stateMutex.Unlock() - return nil - } - - linear, angular := computeNextPower(&b.state, av, b.logger) - - b.stateMutex.Unlock() - return b.setPowerInternal(ctx, linear, angular) -} - -func computeNextPower(state *boatState, angularVelocity spatialmath.AngularVelocity, logger golog.Logger) (r3.Vector, r3.Vector) { - linear := state.lastPowerLinear - angular := state.lastPowerAngular - - angularDiff := angularVelocity.Z - state.velocityAngularGoal.Z - - if math.Abs(angularDiff) > 1 { - delta := angularDiff / 360 - for math.Abs(delta) < .01 { - delta *= 2 - } - - angular.Z -= delta * 10 - angular.Z = math.Max(-1, angular.Z) - angular.Z = math.Min(1, angular.Z) - } - - linear.Y = state.velocityLinearGoal.Y // TEMP - linear.X = state.velocityLinearGoal.X // TEMP - - if logger != nil && true { - logger.Debugf( - "computeNextPower last: %0.2f %0.2f %0.2f goal v: %0.2f %0.2f %0.2f av: %0.2f"+ - " -> %0.2f %0.2f %0.2f", - state.lastPowerLinear.X, - state.lastPowerLinear.Y, - state.lastPowerAngular.Z, - state.velocityLinearGoal.X, - state.velocityLinearGoal.Y, - state.velocityAngularGoal.Z, - angularVelocity.Z, - linear.X, linear.Y, angular.Z, - ) - } - - return linear, angular -} - -func (b *boat) SetVelocity(ctx context.Context, linear, angular r3.Vector, extra map[string]interface{}) error { - b.logger.Debugf("SetVelocity %v %v", linear, angular) - _, done := b.opMgr.New(ctx) - defer done() - - b.stateMutex.Lock() - - if !b.state.threadStarted { - err := b.startVelocityThread() - if err != nil { - return err - } - b.state.threadStarted = true - } - - b.state.velocityControlled = true - b.state.velocityLinearGoal = linear - b.state.velocityAngularGoal = angular - b.stateMutex.Unlock() - - return nil -} - -func (b *boat) SetPower(ctx context.Context, linear, angular r3.Vector, extra map[string]interface{}) error { - b.logger.Debugf("SetPower %v %v", linear, angular) - ctx, done := b.opMgr.New(ctx) - defer done() - - b.stateMutex.Lock() - b.state.velocityControlled = false - b.stateMutex.Unlock() - - return b.setPowerInternal(ctx, linear, angular) -} - -func (b *boat) setPowerInternal(ctx context.Context, linear, angular r3.Vector) error { - power, err := b.cfg.computePower(linear, angular) - if err != nil { - return err - } - - for idx, p := range power { - err := b.motors[idx].SetPower(ctx, p, nil) - if err != nil { - return multierr.Combine(b.Stop(ctx, nil), err) - } - if ctx.Err() != nil { - return ctx.Err() - } - } - - b.stateMutex.Lock() - b.state.lastPower = power - b.state.lastPowerLinear = linear - b.state.lastPowerAngular = angular - b.stateMutex.Unlock() - - return nil -} - -func (b *boat) Stop(ctx context.Context, extra map[string]interface{}) error { - b.stateMutex.Lock() - b.state.velocityLinearGoal = r3.Vector{} - b.state.velocityAngularGoal = r3.Vector{} - b.stateMutex.Unlock() - - b.opMgr.CancelRunning(ctx) - var err error - for _, m := range b.motors { - err = multierr.Combine(m.Stop(ctx, nil), err) - } - return err -} - -func (b *boat) Width(ctx context.Context) (int, error) { - return int(b.cfg.WidthMM), nil -} - -func (b *boat) IsMoving(ctx context.Context) (bool, error) { - for _, m := range b.motors { - isMoving, _, err := m.IsPowered(ctx, nil) - if err != nil { - return false, err - } - if isMoving { - return true, err - } - } - return false, nil -} - -func (b *boat) Close(ctx context.Context) error { - if b.cancel != nil { - b.cancel() - b.cancel = nil - b.waitGroup.Wait() - } - return b.Stop(ctx, nil) -} diff --git a/components/base/boat/base_test.go b/components/base/boat/base_test.go deleted file mode 100644 index d30a37d3507..00000000000 --- a/components/base/boat/base_test.go +++ /dev/null @@ -1,51 +0,0 @@ -package boat - -import ( - "testing" - - "github.com/golang/geo/r3" - "go.viam.com/test" - - "go.viam.com/rdk/spatialmath" -) - -func TestComputeNextPower(t *testing.T) { - _, a := computeNextPower( - &boatState{ - velocityAngularGoal: r3.Vector{Z: 10}, - }, - spatialmath.AngularVelocity{}, - nil, - ) - test.That(t, a.Z, test.ShouldAlmostEqual, .2777777, .01) - - _, a = computeNextPower( - &boatState{ - velocityAngularGoal: r3.Vector{Z: -10}, - }, - spatialmath.AngularVelocity{}, - nil, - ) - test.That(t, a.Z, test.ShouldAlmostEqual, -.2777777, .01) - - _, a2 := computeNextPower( - &boatState{ - lastPowerAngular: r3.Vector{Z: .3}, - velocityAngularGoal: r3.Vector{Z: 45}, - }, - spatialmath.AngularVelocity{Z: 30}, - nil, - ) - test.That(t, a2.Z, test.ShouldBeGreaterThan, a.Z) - test.That(t, a2.Z, test.ShouldBeGreaterThan, .3) - - _, a = computeNextPower( - &boatState{ - lastPowerAngular: r3.Vector{Z: -.2}, - velocityAngularGoal: r3.Vector{Z: 45}, - }, - spatialmath.AngularVelocity{Z: -30}, - nil, - ) - test.That(t, a.Z, test.ShouldBeGreaterThan, 0) -} diff --git a/components/base/boat/boat_config.go b/components/base/boat/boat_config.go deleted file mode 100644 index 5846cf52b86..00000000000 --- a/components/base/boat/boat_config.go +++ /dev/null @@ -1,162 +0,0 @@ -//go:build !windows - -package boat - -import ( - "fmt" - "math" - - "github.com/go-nlopt/nlopt" - "github.com/golang/geo/r3" - "go.uber.org/multierr" - "gonum.org/v1/gonum/mat" - - "go.viam.com/rdk/resource" -) - -type boatConfig struct { - resource.TriviallyValidateConfig - Motors []motorConfig - LengthMM float64 `json:"length_mm"` - WidthMM float64 `json:"width_mm"` - IMU string -} - -func (bc *boatConfig) maxWeights() motorWeights { - var max motorWeights - for _, mc := range bc.Motors { - w := mc.computeWeights(math.Hypot(bc.WidthMM, bc.LengthMM)) - max.linearX += math.Abs(w.linearX) - max.linearY += math.Abs(w.linearY) - max.angular += math.Abs(w.angular) - } - return max -} - -// examples: -// currentVal=2 otherVal=1, currentGoal=1, otherGoal=1 = 1 -// currentVal=-2 otherVal=1, currentGoal=1, otherGoal=1 = -1 - -func goalScale(currentVal, otherVal, currentGoal, otherGoal float64) float64 { - // near 0, do nothing - if math.Abs(currentGoal) < .05 || math.Abs(otherGoal) < .05 { - return currentVal - } - - ratioGoal := math.Abs(currentGoal / otherGoal) - ratioCur := math.Abs(currentVal / otherVal) - - if ratioCur > ratioGoal { - currentVal = otherVal * ratioGoal - } - - return currentVal -} - -func (bc *boatConfig) computeGoal(linear, angular r3.Vector) motorWeights { - w := bc.maxWeights() - w.linearX *= linear.X - w.linearY *= linear.Y - w.angular *= angular.Z - - w.linearX = goalScale(w.linearX, w.linearY, linear.X, linear.Y) - w.linearY = goalScale(w.linearY, w.linearX, linear.Y, linear.X) - - // we ignore angular as the ratios don't really make sense there - - return w -} - -func (bc *boatConfig) weights() []motorWeights { - res := make([]motorWeights, len(bc.Motors)) - for idx, mc := range bc.Motors { - w := mc.computeWeights(math.Hypot(bc.WidthMM, bc.LengthMM)) - res[idx] = w - } - return res -} - -func (bc *boatConfig) weightsAsMatrix() *mat.Dense { - m := mat.NewDense(3, len(bc.Motors), nil) - - for idx, w := range bc.weights() { - m.Set(0, idx, w.linearX) - m.Set(1, idx, w.linearY) - m.Set(2, idx, w.angular) - } - - return m -} - -func (bc *boatConfig) computePowerOutputAsMatrix(powers []float64) mat.Dense { - if len(powers) != len(bc.Motors) { - panic(fmt.Errorf("powers wrong length got: %d should be: %d", len(powers), len(bc.Motors))) - } - var out mat.Dense - - out.Mul(bc.weightsAsMatrix(), mat.NewDense(len(powers), 1, powers)) - - return out -} - -func (bc *boatConfig) computePowerOutput(powers []float64) motorWeights { - out := bc.computePowerOutputAsMatrix(powers) - - return motorWeights{ - linearX: out.At(0, 0), - linearY: out.At(1, 0), - angular: out.At(2, 0), - } -} - -// returns an array of power for each motors -// forwardPercent: -1 -> 1 percent of power in which you want to move laterally -// -// note only x & y are relevant. y is forward back, x is lateral -// -// angularPercent: -1 -> 1 percent of power you want applied to move angularly -// -// note only z is relevant here -func (bc *boatConfig) computePower(linear, angular r3.Vector) ([]float64, error) { - goal := bc.computeGoal(linear, angular) - opt, err := nlopt.NewNLopt(nlopt.GN_DIRECT, 6) - if err != nil { - return nil, err - } - defer opt.Destroy() - - mins := []float64{} - maxs := []float64{} - - for range bc.Motors { - mins = append(mins, -1) - maxs = append(maxs, 1) - } - - err = multierr.Combine( - opt.SetLowerBounds(mins), - opt.SetUpperBounds(maxs), - - opt.SetStopVal(.002), - opt.SetMaxTime(.25), - ) - if err != nil { - return nil, err - } - - myfunc := func(x, gradient []float64) float64 { - total := bc.computePowerOutput(x) - return total.diff(goal) - } - - err = opt.SetMinObjective(myfunc) - if err != nil { - return nil, err - } - powers, _, err := opt.Optimize(make([]float64, 6)) - if err != nil { - return nil, err - } - - return powers, nil -} diff --git a/components/base/boat/boat_config_test.go b/components/base/boat/boat_config_test.go deleted file mode 100644 index ce6fc8e3e37..00000000000 --- a/components/base/boat/boat_config_test.go +++ /dev/null @@ -1,178 +0,0 @@ -package boat - -import ( - "testing" - - "github.com/golang/geo/r3" - "go.viam.com/test" -) - -var testMotorConfig = []motorConfig{ - {Name: "starboard-rotation", XOffsetMM: 300, YOffsetMM: 0, AngleDegrees: 0, Weight: 1}, - {Name: "port-rotation", XOffsetMM: -300, YOffsetMM: 0, AngleDegrees: 0, Weight: 1}, - {Name: "forward", XOffsetMM: 0, YOffsetMM: -300, AngleDegrees: 0, Weight: 1}, - {Name: "reverse", XOffsetMM: 0, YOffsetMM: 300, AngleDegrees: 180, Weight: 1}, - {Name: "starboard-lateral", XOffsetMM: 450, YOffsetMM: 0, AngleDegrees: 90, Weight: 1}, - {Name: "port-lateral", XOffsetMM: -450, YOffsetMM: 0, AngleDegrees: -90, Weight: 1}, -} - -func TestBoatConfig(t *testing.T) { - cfg := boatConfig{ - Motors: testMotorConfig, - LengthMM: 500, - WidthMM: 500, - } - - max := cfg.maxWeights() - test.That(t, max.linearY, test.ShouldAlmostEqual, 4, testTheta) - test.That(t, max.linearX, test.ShouldAlmostEqual, 2, testTheta) - test.That(t, max.angular, test.ShouldAlmostEqual, .845, testTheta) // TODO(erh): is this right? - - g := cfg.computeGoal(r3.Vector{0, 1, 0}, r3.Vector{}) - test.That(t, g.linearY, test.ShouldAlmostEqual, 4) - - g = cfg.computeGoal(r3.Vector{1, 1, 0}, r3.Vector{}) - test.That(t, g.linearX, test.ShouldAlmostEqual, 2) - test.That(t, g.linearY, test.ShouldAlmostEqual, 2) - - g = cfg.computeGoal(r3.Vector{.2, 1, 0}, r3.Vector{}) - test.That(t, g.linearX, test.ShouldAlmostEqual, .4) - test.That(t, g.linearY, test.ShouldAlmostEqual, 2) - - g = cfg.computeGoal(r3.Vector{0, 1, 0}, r3.Vector{Z: .05}) - test.That(t, g.linearX, test.ShouldAlmostEqual, 0) - test.That(t, g.linearY, test.ShouldBeGreaterThan, .9) - test.That(t, g.angular, test.ShouldBeLessThan, .1) - - powers, err := cfg.computePower(r3.Vector{0, 1, 0}, r3.Vector{}) - test.That(t, err, test.ShouldBeNil) - test.That(t, powers[0], test.ShouldAlmostEqual, 1, testTheta) - test.That(t, powers[1], test.ShouldAlmostEqual, 1, testTheta) - test.That(t, powers[2], test.ShouldAlmostEqual, 1, testTheta) - test.That(t, powers[3], test.ShouldAlmostEqual, -1, testTheta) - test.That(t, powers[4], test.ShouldAlmostEqual, 0, testTheta) - test.That(t, powers[5], test.ShouldAlmostEqual, 0, testTheta) - - powers, err = cfg.computePower(r3.Vector{0, -1, 0}, r3.Vector{}) - test.That(t, err, test.ShouldBeNil) - test.That(t, powers[0], test.ShouldAlmostEqual, -1, testTheta) - test.That(t, powers[1], test.ShouldAlmostEqual, -1, testTheta) - test.That(t, powers[2], test.ShouldAlmostEqual, -1, testTheta) - test.That(t, powers[3], test.ShouldAlmostEqual, 1, testTheta) - test.That(t, powers[4], test.ShouldAlmostEqual, 0, testTheta) - test.That(t, powers[5], test.ShouldAlmostEqual, 0, testTheta) - - powers, err = cfg.computePower(r3.Vector{0, 0, 0}, r3.Vector{Z: 1}) - test.That(t, err, test.ShouldBeNil) - test.That(t, powers[0], test.ShouldAlmostEqual, 1, testTheta) - test.That(t, powers[1], test.ShouldAlmostEqual, -1, testTheta) - test.That(t, powers[2], test.ShouldAlmostEqual, 0, testTheta) - test.That(t, powers[3], test.ShouldAlmostEqual, 0, testTheta) - test.That(t, powers[4], test.ShouldAlmostEqual, 0, testTheta) - test.That(t, powers[5], test.ShouldAlmostEqual, 0, testTheta) - - powers, err = cfg.computePower(r3.Vector{0, 0, 0}, r3.Vector{Z: -1}) - test.That(t, err, test.ShouldBeNil) - test.That(t, powers[0], test.ShouldAlmostEqual, -1, testTheta) - test.That(t, powers[1], test.ShouldAlmostEqual, 1, testTheta) - test.That(t, powers[2], test.ShouldAlmostEqual, 0, testTheta) - test.That(t, powers[3], test.ShouldAlmostEqual, 0, testTheta) - test.That(t, powers[4], test.ShouldAlmostEqual, 0, testTheta) - test.That(t, powers[5], test.ShouldAlmostEqual, 0, testTheta) - - t.Run("matrix-base", func(t *testing.T) { - m := cfg.computePowerOutputAsMatrix([]float64{0, 0, 0, 0, 0, 0}) - r, c := m.Dims() - test.That(t, 3, test.ShouldEqual, r) - test.That(t, 1, test.ShouldEqual, c) - - for idx, w := range cfg.weights() { - powers := make([]float64, 6) - powers[idx] = 1 - out := cfg.computePowerOutput(powers) - test.That(t, w, test.ShouldResemble, out) - } - }) - - l, a := r3.Vector{1, 0, 0}, r3.Vector{} - powers, err = cfg.computePower(l, a) - test.That(t, err, test.ShouldBeNil) - test.That(t, cfg.computePowerOutput(powers), weightsAlmostEqual, cfg.computeGoal(l, a)) - - l, a = r3.Vector{0, 1, 0}, r3.Vector{} - powers, err = cfg.computePower(l, a) - test.That(t, err, test.ShouldBeNil) - test.That(t, cfg.computePowerOutput(powers), weightsAlmostEqual, cfg.computeGoal(l, a)) - - l, a = r3.Vector{-.5, 1, 0}, r3.Vector{} - powers, err = cfg.computePower(l, a) - test.That(t, err, test.ShouldBeNil) - test.That(t, cfg.computePowerOutput(powers), weightsAlmostEqual, cfg.computeGoal(l, a)) - - l, a = r3.Vector{}, r3.Vector{Z: .125} - powers, err = cfg.computePower(l, a) - test.That(t, err, test.ShouldBeNil) - test.That(t, cfg.computePowerOutput(powers), weightsAlmostEqual, cfg.computeGoal(l, a)) - - l, a = r3.Vector{X: 1, Y: 1}, r3.Vector{} - powers, err = cfg.computePower(l, a) - test.That(t, err, test.ShouldBeNil) - test.That(t, cfg.computePowerOutput(powers), weightsAlmostEqual, cfg.computeGoal(l, a)) - test.That(t, powers[0]+powers[1]+powers[2]+-1*powers[3], test.ShouldAlmostEqual, powers[4]+-1*powers[5], .01) - - l, a = r3.Vector{X: .2, Y: 1}, r3.Vector{} - powers, err = cfg.computePower(l, a) - test.That(t, err, test.ShouldBeNil) - test.That(t, cfg.computePowerOutput(powers), weightsAlmostEqual, cfg.computeGoal(l, a)) - - l, a = r3.Vector{X: -1, Y: -1}, r3.Vector{} - powers, err = cfg.computePower(l, a) - test.That(t, err, test.ShouldBeNil) - test.That(t, cfg.computePowerOutput(powers), weightsAlmostEqual, cfg.computeGoal(l, a)) - test.That(t, powers[0]+powers[1]+powers[2]+-1*powers[3], test.ShouldAlmostEqual, powers[4]+-1*powers[5], .01) - - l, a = r3.Vector{X: -.9, Y: -.9}, r3.Vector{} - powers, err = cfg.computePower(l, a) - test.That(t, err, test.ShouldBeNil) - test.That(t, cfg.computePowerOutput(powers), weightsAlmostEqual, cfg.computeGoal(l, a)) - test.That(t, powers[0]+powers[1]+powers[2]+-1*powers[3], test.ShouldAlmostEqual, powers[4]+-1*powers[5], .01) - - l, a = r3.Vector{X: 0, Y: 1}, r3.Vector{Z: .05} - powers, err = cfg.computePower(l, a) - test.That(t, err, test.ShouldBeNil) - test.That(t, powers[4]+powers[5], test.ShouldAlmostEqual, 0, .0001) - test.That(t, powers[0]+powers[1]+powers[2]+-1*powers[3], test.ShouldBeGreaterThan, 3.5) -} - -func weightsAlmostEqual(actual interface{}, expected ...interface{}) string { - a := actual.(motorWeights) - e := expected[0].(motorWeights) - - if s := test.ShouldAlmostEqual(a.linearX, e.linearX, testTheta); s != "" { - return "x: " + s - } - - if s := test.ShouldAlmostEqual(a.linearY, e.linearY, testTheta); s != "" { - return "y: " + s - } - - if s := test.ShouldAlmostEqual(a.angular, e.angular, testTheta); s != "" { - return "angular: " + s - } - - return "" -} - -func BenchmarkComputePower(b *testing.B) { - cfg := boatConfig{ - Motors: testMotorConfig, - LengthMM: 500, - WidthMM: 500, - } - - b.ResetTimer() - - for n := 0; n < b.N; n++ { - cfg.computePower(r3.Vector{0, 1, 0}, r3.Vector{}) - } -} diff --git a/components/base/boat/motor.go b/components/base/boat/motor.go deleted file mode 100644 index d069c8a81a4..00000000000 --- a/components/base/boat/motor.go +++ /dev/null @@ -1,55 +0,0 @@ -package boat - -import ( - "math" - - "go.viam.com/rdk/utils" -) - -type motorWeights struct { - linearX float64 - linearY float64 - angular float64 -} - -func (mw *motorWeights) diff(other motorWeights) float64 { - return math.Sqrt(math.Pow(mw.linearX-other.linearX, 2) + - math.Pow(mw.linearY-other.linearY, 2) + - math.Pow(mw.angular-other.angular, 2)) -} - -type motorConfig struct { - Name string - XOffsetMM float64 `json:"x_offset_mm"` - YOffsetMM float64 `json:"y_offset_mm"` - AngleDegrees float64 `json:"angle_degs"` // 0 is thrusting forward, 90 is thrusting to starboard, or positive x - Weight float64 -} - -// percentDistanceFromCenterOfMass: if the boat is a circle with a radius of 5m, -// this is the distance from center in m / 5m. -func (mc *motorConfig) computeWeights(radius float64) motorWeights { - x := math.Sin(utils.DegToRad(mc.AngleDegrees)) * mc.Weight - y := math.Cos(utils.DegToRad(mc.AngleDegrees)) * mc.Weight - - angleFromCenter := 0.0 - if mc.YOffsetMM == 0 { - if mc.XOffsetMM > 0 { - angleFromCenter = 90 - } else if mc.XOffsetMM < 0 { - angleFromCenter = -90 - } - } else { - angleFromCenter = utils.RadToDeg(math.Atan(mc.XOffsetMM / mc.YOffsetMM)) - } - - percentDistanceFromCenterOfMass := math.Hypot(mc.XOffsetMM, mc.YOffsetMM) / radius - - angleOffset := mc.AngleDegrees - angleFromCenter - - return motorWeights{ - linearX: x, - linearY: y, - angular: -1 * percentDistanceFromCenterOfMass * mc.Weight * math.Sin(utils.DegToRad(angleOffset)), - } -} diff --git a/components/base/boat/motor_test.go b/components/base/boat/motor_test.go deleted file mode 100644 index 80964682568..00000000000 --- a/components/base/boat/motor_test.go +++ /dev/null @@ -1,81 +0,0 @@ -package boat - -import ( - "fmt" - "math" - "testing" - - "go.viam.com/test" -) - -const testTheta = .01 - -func TestMotorWeights(t *testing.T) { - type d struct { - cfg motorConfig - res motorWeights - } - - tests := []d{ - { - motorConfig{ - XOffsetMM: 0, - YOffsetMM: -10, - AngleDegrees: 0, - Weight: 1, - }, - motorWeights{0, 1, 0}, - }, - { - motorConfig{ - XOffsetMM: 0, - YOffsetMM: -10, - AngleDegrees: 180, - Weight: 1, - }, - motorWeights{0, -1, 0}, - }, - { - motorConfig{ - AngleDegrees: 45, - Weight: math.Sqrt(2), - }, - motorWeights{1, 1, 0}, - }, - { - motorConfig{ - XOffsetMM: -10, - YOffsetMM: -10, - AngleDegrees: 45, - Weight: math.Sqrt(2), - }, - motorWeights{1, 1, 0}, - }, - - { - motorConfig{ - AngleDegrees: 1, // this should be almost entirely linearY - Weight: 1, - }, - motorWeights{.017, .99, 0}, - }, - { - motorConfig{ - XOffsetMM: 0, - YOffsetMM: -10, - AngleDegrees: 90, - Weight: 1, - }, - motorWeights{1, 0, -1}, - }, - } - - for _, x := range tests { - t.Run(fmt.Sprintf("%#v", x), func(t *testing.T) { - w := x.cfg.computeWeights(10) - test.That(t, w.angular, test.ShouldAlmostEqual, x.res.angular, testTheta) - test.That(t, w.linearX, test.ShouldAlmostEqual, x.res.linearX, testTheta) - test.That(t, w.linearY, test.ShouldAlmostEqual, x.res.linearY, testTheta) - }) - } -} diff --git a/components/base/register/register.go b/components/base/register/register.go index 22451140c85..d618272236b 100644 --- a/components/base/register/register.go +++ b/components/base/register/register.go @@ -4,7 +4,6 @@ package register import ( // register bases. _ "go.viam.com/rdk/components/base/agilex" - _ "go.viam.com/rdk/components/base/boat" _ "go.viam.com/rdk/components/base/fake" _ "go.viam.com/rdk/components/base/wheeled" )