Need help resolving steady-state error in arm feedforward

I am implementing feedforward for the rotation component of a pink arm but I am encountering steady-state error with a PID controller. Since the length of the arm (which impacts the effect of gravity on the rotation) varies, I am not using the ArmFeedforward class from WPILib, but I am calculating the feedforward using the calculation on line 58. All of the units in my feedforward implementation are in volts. I am testing using a simulated arm because I do not have around-the-clock access to an actual robot.

Previously, with only a PID controller, the simulated arm was able to consistently rotate until the simulated arm angle was equal to the setpoint arm angle. The addition of gravity compensation using the feedforward voltage calculator caused this feature to break, with the simulated arm angle rotating to a few degrees above the setpoint angle and being unable to progress further. Here is the whole Shuffleboard tab containing the data from the simulated arm when it is stuck, and the lists containing the arm position and setpoint position:

Here’s a visualization of the arm position when it is stuck, using a mechanism:


The implementation of the arm feedforward that causes this behavior is this: the feedfoward calculates the voltage needed to resist the force of gravity at the current position (e.g. keep the arm at that position), then the PID controller calculates a voltage required to move the arm towards the setpoint, then these voltages are added together (which accomplishes staying at the setpoint if not at the setpoint, otherwise moving towards the setpoint) and sent to the simulated motor.

The calculation to resist the force of gravity is at:

The constant in there was found by setting the arm’s input rotation voltage to zero, then increasing the kG variable until the arm stayed still in one position. This is the same methodology from the WPILib article on tuning an arm position controller.

The PID calculation (for the simulated arm) is at:

The summation of the voltages and using the voltage with the simulated arm is at: and

What I am stuck on is the fact that the feedforward calculation produces the correct amount of voltage to hold the simulated arm at an angle when input voltage is zero (and the simulated brake is disabled, which was separately tested to be okay), and that the PID controller produces a value that is able to reduce the error by a considerable amount before becoming stuck at the steady-state position a few degrees above the setpoint. Here the values I am getting for each calculation when the arm is stuck at the steady-state position:


  • inputVoltage is the voltage from the PID controller, attempting to match the setpoint
  • voltageToOvercomeGravity is what the feedforward controller calculates is the voltage needed to resist gravity (hold the arm at the angle) for the arm’s position
  • outputVoltage is the voltage sent to the motor, controlling the arm angle

Since the output voltage is less than the voltage needed to overcome gravity, shouldn’t the arm angle decrease, instead of staying still?

Also, it doesn’t seem like a coincidence that the stuck state occurs when the input and output voltages are opposites. What does that have to do with the angle of the arm not changing, when only the output voltage is being sent to the motor?


Why is your feedforward function accepting voltage as an input?

I will eventually be implementing feedforward for static friction, so it needs to take direction to apply the voltage as a parameter. It is unused in this version.

This would probably be better-achieved by basing that on the direction of the desired velocity.

Why not use the WPILib ArmFeedforward?

The voltage is analagous to the “velocity” (change in position) in this case, but that could be made more clear.

As the arm extends, the voltage needed to overcome gravity also increases. Since kG can’t be changed at runtime, one ArmFeedforward can’t work. It was easier to extract the gravity term into a separate class.

Just copy the implementation of the whole ArmFeedforward class and give it some extra parameters, rather than extracting pieces of it.

A more-accurate kG identification routine is to find the voltages that cause incremental motion in both the up and down direction, and take their average. You can take the half the difference instead of the average to find kS. There is a buffer of control signal values around kG, of radius kS, within which the arm will not move due to stiction.

What should “incremental” look like? <1deg/s? <0.5deg/s?

We experimentally determined Kg and Ks on our 2 joint arm by performing a sweep where it would attempt to hold still at a sequence of positions, using a P loop and the theoretical values for Kg. Then we compare the holding voltage + the actual position held by that voltage, and perform regression to fit the equation V = Kg*cos(theta) + Ks*sign(v). For your telescoping arm, I’d recommend finding Kg_min and Kg_max when fully retracted and fully extended, respectively, and Kg should in theory be the linear interpolation between Kg(extension = 0) = Kg_min and Kg(extension = max) = Kg_max.

As for why it didn’t move when voltageToOvercomeGravity > outputVoltage, you’ll have to do a bit more digging in the simulator. It seems that the FF’s expected voltageToOvercomeGravity doesn’t match what the simulator thinks it should be.

I recalculated kG in the simulator and that prevented the arm from getting stuck. Not sure why the kG value from before was off.

The smallest motion you can detect.