Add ArmFeedforward calculate() overload that takes current and next velocity instead of acceleration

[calcmogul]

Is your feature request related to a problem? Please describe.
Motion profile classes don't include acceleration in their provided state (because it's not a state). The feedforward calculate() methods require acceleration though.

Describe the solution you'd like
We've added a calculate() overload to SimpleMotorFeedforward that takes current and next velocity instead. We should do that for all our other feedforward classes as well.

The two overloads would be calculate(velocity, acceleration) and calculate(currentVelocity, nextVelocity, dt).

Describe alternatives you've considered
Adding acceleration to the motion profile isn't ideal because the control input is inherently discrete; the acceleration continuously varies across the timestep instead of being constant. The discrete version of calculate() is more accurate. The next velocity can be obtained by sampling the motion profile one timestep into the future.

[calcmogul]

ElevatorFeedforward's model is affine, which is analytically invertible.

dx/dt = Ax + Bu + c
dx/dt = Ax + B(u + B⁺c)
xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ

Solve for uₖ.

B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ

For an elevator with the model dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka).

uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka))
uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka)
uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks

ArmFeedforward's model is nonlinear though. One way to implement ArmFeedforward's new calculate() overload would be to integrate the dynamics with rk4(), then use Newton's method to find the input u that minimizes the difference between the current and next reference.

[narmstro2020]

So one would need to perform rk4() on this component B⁺cₖ
where cₖ = -(Kg * cos(x)/Ka + Ks/Ka).
So rk4() on Kg* cos(x) at the current velocity.
Then take the result from rk4() and perform newton's method for next and current.
Is there a method for Newton's method in the lib?

[narmstro2020]

For the nonlinear portion B_d.solve(d) where d is the column vector (0, -kG cos(x) / kA)

Then rk4() this function of x at current velocity?

[narmstro2020]

Something like this

    var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
    var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);

    var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
    var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);



    DoubleFunction<Double> dynamics = (posRads) -> {
        Matrix<N1, N1> d = Matrix.mat(Nat.N1(), Nat.N1())
        .fill(-kg * Math.cos(posRads) / ka);
      var  m_A = plant.getA();
      var  m_B = plant.getB();
      var discABpair = Discretization.discretizeAB(m_A, m_B, dtSeconds);
      Matrix<N1, N1> discB = discABpair.getSecond();
      var discD = m_B.solve(d);
      return discD.get(0, 0);
    };

    double integral = NumericalIntegration.rk4(dynamics, currentVelocity, dtSeconds);

[calcmogul]

That computation for discrete d doesn't seem right; the nonlinear dynamics are not seperable. Also, passing discrete dynamics to RK4 is invalid; RK4 integrates continuous dynamics.

https://github.com/calcmogul/allwpilib/tree/wpimath-add-sleipnir can solve the least-squares problem I mentioned. I didn't make a PR yet because we didn't have a use case, and it's not very good at nonlinear problems. In any case, the formulation would look like this:

#include <sleipnir/optimization/OptimizationProblem.hpp>

// Init xₖ and xₖ₊₁ here
double x_k;
double x_k1;

sleipnir::OptimizationProblem problem;
auto u_k = problem.DecisionVariable();
problem.Minimize((x_k1 - RK4(f, x_k, u_k, dt)).transpose() * (x_k1 - RK4(f, x_k, u_k, dt)));
problem.Solve();

// u_k.Value() contains solution.

Where f(x, u) is the acceleration dynamics. If it has trouble converging, forward Euler could be used to make the cost function better behaved, though the solution may be less accurate.

A JNI would be needed to implement the Java version. A native Java implementation of Newton's method with EJML would be slow.

[narmstro2020]

So would using the velocity, acceleration method be better in the long run for arm.

[calcmogul]

But the whole point of this issue is to implement a more accurate method with arguments the user has more readily available.

[narmstro2020]

That's fair. I just don't think I can write a JNI :)
But its good for someone else to do.

[narmstro2020]

What would the full form be of f(x, u)

[calcmogul]

For an arm, acceleration = -Kv/Ka * velocity + 1/Ka * voltage - Ks/Ka * sgn(velocity) - Kg/Ka * cos(position). Let x = [position; velocity] and u = [voltage].

        [0     1  ]    [ 0  ]    [                0                 ]
dx/dt = [0  -Kv/Ka]x + [1/Ka]u + [-Ks/Ka sgn(x[1]) - Kg/Ka cos(x[0])]

[narmstro2020]

Another thought on this. Would it be possible to use a small angle approximation for the cosine to make the optimization take less time or is that just substituting one nonlinear for another.

[calcmogul]

Sounds like premature optimization since the optimization hasn't yet been demonstrated to take too long.

[narmstro2020]

I've been thinking about the method signature for the arm.

Would an appropriate set of arguments for calculate be (currentPos, nextPos, currentVel, nextVel, dtSeconds).

We didn't need position for the simple motor or the elevator feedforwards as they don't have a dependency on position, whereas the arm does.

[calcmogul]

I'd do calculate(currentPos, currentVel, nextPos, nextVel, dtSeconds) to reflect the state vector grouping.

[narmstro2020]

gotcha.

[narmstro2020]

Is this a good start

public double calculate(double currentPos, double currentVel, double nextPos, double nextVel, double dtSeconds) {

    var r = Matrix.mat(Nat.N2(), Nat.N1()).fill(currentPos, currentVel);
    var nextR = Matrix.mat(Nat.N2(), Nat.N1()).fill(nextPos, nextVel);

    Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka);
    Matrix<N2, N1> B = Matrix.mat(Nat.N2(), Nat.N1()).fill(0, 1 / ka);
    Matrix<N2, N1> C = Matrix.mat(Nat.N2(), Nat.N1()).fill(0, Math.signum(r.get(1, 0)) * -ks / ka - Math.cos(r.get(0,0)) * kg/ka);
    BiFunction<Matrix<N2, N1>, Matrix<N1, N1>, Matrix<N2, N1>> f = (x, u) -> A.times(x).plus(B.times(u)).plus(C);

[narmstro2020]

And then something like this

I know the name functionToFindZeroFor is ridiculous

    var r = Matrix.mat(Nat.N2(), Nat.N1()).fill(currentPos, currentVel);
    var nextR = Matrix.mat(Nat.N2(), Nat.N1()).fill(nextPos, nextVel);

    Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka);
    Matrix<N2, N1> B = Matrix.mat(Nat.N2(), Nat.N1()).fill(0, 1 / ka);
    Matrix<N2, N1> C = Matrix.mat(Nat.N2(), Nat.N1()).fill(0, Math.signum(r.get(1, 0)) * -ks / ka - Math.cos(r.get(0,0)) * kg/ka);
    BiFunction<Matrix<N2, N1>, Matrix<N1, N1>, Matrix<N2, N1>> f = (xk, uk) -> A.times(xk).plus(B.times(uk)).plus(C);

    Function<Matrix<N1, N1>, Matrix<N2, N1>> k1 = (uk) -> f.apply(r, uk);
    Function<Matrix<N1, N1>, Matrix<N2, N1>> k2 = (uk) -> f.apply(r.plus(k1.apply(uk).times(0.5 * dtSeconds)), uk);
    Function<Matrix<N1, N1>, Matrix<N2, N1>> k3 = (uk) -> f.apply(r.plus(k2.apply(uk).times(0.5 * dtSeconds)), uk);
    Function<Matrix<N1, N1>, Matrix<N2, N1>> k4 = (uk) -> f.apply(r.plus(k3.apply(uk).times(dtSeconds)), uk);
    Function<Matrix<N1, N1>, Matrix<N2, N1>> functionToFindZeroFor = (uk) -> {
        var part1 = k1.apply(uk).times(dtSeconds / 6);
        var part2 = k2.apply(uk).times(dtSeconds / 3);
        var part3 = k3.apply(uk).times(dtSeconds / 3);
        var part4 = k4.apply(uk).times(dtSeconds / 6);
        var sum = part1.plus(part2.plus(part3.plus(part4)));
        return r.plus(sum).minus(nextR);
    };

[calcmogul]

wpimath has a function for RK4, if that's what you're trying to do (I can't tell because Java sucks at making math legible).

[narmstro2020]

Wouldn't I be trying to find the uk that makes the functionToFindZeroFor zero. I didn't think rk4 would be useful since I don't know u

[calcmogul]

That code is so verbose, I can't tell what it does or whether it's correct.

[narmstro2020]

This would be for the calculate method.

I used Theorem 7.9.2 in your book with xk_1 as nextR and xk as r to create a function to find the zero for.

[narmstro2020]

So I could solve the the input u which I guess would be the next step in the calculate method.

[calcmogul]

I'm still not seeing it. Could you show your work if this is actually different from what rk4() does?

[narmstro2020]

Referencing Theorem 7.9.2

I'll use xk_1 as xsub(k+1)
and xk as xsubk

nextR is xk_1
r is xk

dtseconds is h

So using

xk_1 = xk + (dtSecond / 6) * (k1 + 2 *k2 + 2 * k3 + k4)

k1, k2, k3, k4 are all functions of xk and u. So I plugged r in for xk and into k1, k2, k3, and k4.

Now I have a function strictly of u that connects xk_1 and xk

[calcmogul]

That just sounds like NumericalIntegration.rk4().

[narmstro2020]

Right, but I don't know which u gets me from r to nextR. Doesn't NumericalIntegration.rk4() require that I already know u

[calcmogul]

You're passing uk as an argument to functionToFindZeroFor anyway.

[narmstro2020]

Yes. Wouldn't the output of the calculate method be the uk that is the zero of the functionToFindZeroFor.

My next step would be to find this (these zeroes) somehow.

[narmstro2020]

So I effectively have g(uk) + xk - xk_1 = 0.

Where g(uk) is h/6 ( k1 + 2 k2 + 2 k3 + k4) and the xk has already been plugged in for all the k's

Note: I changed the function to g(uk) to avoid confusion with the main dynamics function f.

[narmstro2020]

Actually let me back up a second.

I would have a function of uk that gives me all possible xk_1 based on the current xk

Then I would have to find the uk that got me closest to nextR

[narmstro2020]

Actually it may never cross the 0 given the inputs and the timestep so minimize?

[narmstro2020]

Okay scratch everything.

    var r = Matrix.mat(Nat.N2(), Nat.N1()).fill(currentPos, currentVel);
    var nextR = Matrix.mat(Nat.N2(), Nat.N1()).fill(nextPos, nextVel);

    Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka);
    Matrix<N2, N1> B = Matrix.mat(Nat.N2(), Nat.N1()).fill(0, 1 / ka);
    Matrix<N2, N1> C = Matrix.mat(Nat.N2(), Nat.N1()).fill(0, Math.signum(r.get(1, 0)) * -ks / ka - Math.cos(r.get(0,0)) * kg/ka);
    BiFunction<Matrix<N2, N1>, Matrix<N1, N1>, Matrix<N2, N1>> f = (xk, uk) -> A.times(xk).plus(B.times(uk)).plus(C);

    Function<Matrix<N1, N1>, Matrix<N2, N1>> nextRFunction = (uk) -> {
        return NumericalIntegration.rk4(
            f, 
            r, 
            uk, 
            dtSeconds);
    };

    Function<Matrix<N1, N1>, Matrix<N2, N1>> nextRFunctionDiff = (uk) -> {
        return nextRFunction.apply(uk).minus(nextR);
    };

[narmstro2020]

Two questions for you. I'm assuming the cost function that you mentinoned above is the dynamics function.

Also would something like this be helpful in finding the solution to the optimization problem.

[calcmogul]

The residual would be xₖ₊₁ − rk4(xₖ, uₖ). Levenberg-Marquardt is one of many methods for minimizing it.

[narmstro2020]

Okay.  I’ll see if I can implement this for the arm this week.  Sent from my iPhoneOn Oct 23, 2023, at 2:34 PM, Tyler Veness ***@***.***> wrote: The residual would be xₖ₊₁ − rk4(xₖ, uₖ). Levenberg-Marquardt is one of many methods for minimizing it. —Reply to this email directly, view it on GitHub, or unsubscribe.You are receiving this because you commented.Message ID: ***@***.***>

[narmstro2020]

So just as an experiment. I'm directly using the Levenberg-Marquardt code from the link.

  public double calculate(double currentPos, double currentVel, double nextPos, double nextVel, double dtSeconds) {

    var r = Matrix.mat(Nat.N2(), Nat.N1()).fill(currentPos, currentVel);
    var nextR = Matrix.mat(Nat.N2(), Nat.N1()).fill(nextPos, nextVel);

    Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka);
    Matrix<N2, N1> B = Matrix.mat(Nat.N2(), Nat.N1()).fill(0, 1 / ka);
    Matrix<N2, N1> C = Matrix.mat(Nat.N2(), Nat.N1()).fill(0,
        Math.signum(r.get(1, 0)) * (-ks / ka) - Math.cos(r.get(0, 0)) * (kg / ka));
    BiFunction<Matrix<N2, N1>, Matrix<N1, N1>, Matrix<N2, N1>> f = (xk, uk) -> A.times(xk).plus(B.times(uk)).plus(C);

    Function<Matrix<N1, N1>, Matrix<N2, N1>> nextRFunction = (uk) -> {
      return NumericalIntegration.rk4(
          f,
          r,
          uk,
          dtSeconds);
    };

    Function<Matrix<N1, N1>, Matrix<N2, N1>> residualFunction = (uk) -> {
      return nextR.minus(nextRFunction.apply(uk));
    };

    ResidualFunction residual = new ResidualFunction() {

      @Override
      public void compute(DMatrixRMaj param, DMatrixRMaj residual) {
        Matrix<N1, N1> uk = Matrix.mat(Nat.N1(), Nat.N1()).fill(param.data[0]);
        Matrix<N2, N1> residualMatrix = residualFunction.apply(uk);
        residual.data = residualMatrix.getData();
      }

      @Override
      public int numFunctions() {
        return 1;
      }

    };

    LevenbergMarquardt lm = new LevenbergMarquardt(1);
    DMatrixRMaj inputOutputMatrix = new DMatrixRMaj(2, 1);
    lm.optimize(residual, inputOutputMatrix);
    return inputOutputMatrix.get(0);

  }

[narmstro2020]

@calcmogul
So I'd like to come back to this problem and don't know how to or if I can add a JNI to the Sleipnir optimization problem.
Are there any alternatives that could work that wouldn't require Java. I thought about creating a mutable matrix library that didn't get bogged down by the garbage collector and then implement the Sleipnir opt problem.

I've thought about using the open source Levenberg-Marquardt code that utilizes EJML.

I just wanted to pick your brain a little on alternate possibilities if any.

[calcmogul]

I'd honestly rather just write a JNI. There's similar wpimath JNIs that can be used as a guide for passing matrices back and forth, and doing anything math-related in C++ is just easier.

[narmstro2020]

Okay. I'll make some attempts at this approach. Be that as it may I'll eventually need to integrate the Sleipnir Optimization problem and its dependencies into WPILIB to make this work.

[narmstro2020]

@calcmogul
Do you know where best to look for an existing JNI for reference? Or at least a set of folders to get me going.

[calcmogul]

Branch I made that imports Sleipnir:
https://github.com/calcmogul/allwpilib/tree/upstream-utils-add-sleipnir

JNI example:
https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java

[narmstro2020]

How do I get your upstream-utils-add-sleipnir branch as a branch in my repo for development. I'm sure there is a git way to do this, but I'm not aware of it.

[narmstro2020]

Nevermind I believe I got it.

[narmstro2020]

@calcmogul
Any problem with me using the branch to start a pull request for the arm feedforward?