Unit 8 · Lesson 4

Arm and Elevator Feedforward

SimpleMotorFeedforward models friction and inertia — the forces a horizontal drivetrain must overcome. Arms and elevators face a third force: gravity. It is always present, changes with position for arms, and cannot be ignored. This lesson introduces ArmFeedforward and ElevatorFeedforward — WPILib's gravity-aware feedforward classes — and explains why the cosine is the key difference between them.

By the end of this lesson, you will:

Explain why SimpleMotorFeedforward is insufficient for arms and elevators
State the physical meaning of kG and how it is measured
Explain why the elevator's gravity term is constant but the arm's gravity term varies with cos(θ)
Use the interactive visualizer to build intuition for how kG·cos(θ) changes across an arm's range of motion
Implement both ArmFeedforward and ElevatorFeedforward in a subsystem with the correct calculate() signature
Run the gravity-only test to validate kG before adding PID

The Problem: Gravity Is Not in SimpleMotorFeedforward

The SimpleMotorFeedforward model — V = kS·sgn(v) + kV·v + kA·a — accounts for friction (kS), back-EMF (kV), and inertia (kA). It does not account for gravity. For a drivetrain moving horizontally, this is fine: gravity is perpendicular to the direction of motion and produces no net torque. For an arm rotating in the vertical plane, gravity exerts a torque on the arm at every angle. For an elevator moving vertically, gravity directly opposes the carriage's motion.

If you use SimpleMotorFeedforward on an arm and command zero velocity — intending the arm to hold its position — the feedforward outputs kS·sgn(0) + kV·0 = 0V. The motor applies no force. Gravity pulls the arm down. Without additional effort from PID (which would have to work overtime to counteract the entire gravitational torque alone), the arm falls.

The fix is a fourth gain: kG — the gravity constant — which adds the voltage required to resist gravity, computed in a geometry-aware way.

ElevatorFeedforward — Constant Gravity

An elevator carriage moves straight up and down. Gravity always opposes upward motion with the same force — the weight of the carriage — regardless of how high the carriage is. There is no geometry to consider. The gravity term is simply a constant voltage added to the feedforward output whenever the elevator is commanded to hold or move.

ElevatorFeedforward formula

V = kS·sgn(v) + kG + kV·v + kA·a

kS·sgn(v) Static friction — same as SimpleMotorFeedforward. Overcomes gearbox and bearing friction in the direction of motion.

kG Gravity constant. Voltage needed to hold the carriage at rest against gravity. Constant — gravity pulls equally hard at every elevator height.

kV·v + kA·a Velocity and acceleration terms — identical in meaning to SimpleMotorFeedforward. These handle the back-EMF and inertia of the carriage motion.

Elevator feedforward — gravity is constant at all heights

Carriage height 50%

kG (V) 0.50 V

Velocity (m/s) 0.0 m/s

Gravity term (kG):

Total V_ff:

kG changes with height?No — always kG

Drag the height slider — notice the gravity arrow and kG contribution stay exactly the same at every height. The carriage is heavier at the bottom than at the top? No — it's the same weight. Gravity pulls equally hard regardless of position on a vertical axis.

ArmFeedforward — Gravity Varies with cos(θ)

An arm pivots around a fixed joint. Its end travels through an arc, and at each angle, gravity has a different leverage on the joint. When the arm is horizontal (0°), gravity has maximum torque — the entire weight of the arm acts at maximum lever arm length. When the arm is vertical (pointing straight up, 90°), gravity acts straight down along the arm — through the pivot — with zero horizontal component, producing zero torque. The cosine of the angle captures this exactly: cos(0°) = 1 (maximum torque), cos(90°) = 0 (zero torque).

ArmFeedforward formula

V = kS·sgn(v) + kG·cos(θ) + kV·v + kA·a

kG·cos(θ) Gravity term. kG is the voltage to hold the arm horizontal (0°, maximum gravity torque). cos(θ) scales this as the angle changes. At 90° (vertical), no gravity compensation is needed. At 180° (arm pointing down), the term is negative — gravity now helps hold the arm in place and the motor must resist it.

θ in radians The arm angle passed to calculate() must be in radians (0 = horizontal, π/2 = straight up). WPILib uses the robotics convention: 0° is horizontal. If your encoder reads in degrees or rotations, convert before calling calculate().

Interactive Arm Gravity Visualizer

Drag the angle slider to see how the gravity contribution changes across the arm's range. Watch how kG·cos(θ) peaks at horizontal and reaches zero at vertical.

ArmFeedforward — kG·cos(θ) gravity compensation

Arm angle (θ) 0°

kG (V) 1.00 V

Velocity (rad/s) 0.0 rad/s

cos(θ):

kG·cos(θ):

Total V_ff:

Drag the angle slider from 0° (horizontal) to 90° (vertical). Watch kG·cos(θ) drop to zero at 90°. Then continue past 90° — the gravity term goes negative, meaning gravity now helps hold the arm and the motor must resist it from falling too fast.

Side-by-Side: The Three Feedforward Classes

Class	Formula	Gravity term	calculate() signature	Use when
SimpleMotorFeedforward	kS·sgn(v) + kV·v + kA·a	None	calculate(velocity)	Horizontal mechanisms: drivetrains, flywheels, conveyors, intakes
ElevatorFeedforward	kS·sgn(v) + kG + kV·v + kA·a	kG (constant)	calculate(velocity)	Vertical linear motion: elevators, linear climbers, vertical slides
ArmFeedforward	kS·sgn(v) + kG·cos(θ) + kV·v + kA·a	kG·cos(θ) (position-varying)	calculate(posRad, velocity)	Rotary arms in vertical plane: scoring arms, intakes that pivot, turrets tilting vertically

⚠️ The critical difference in calculate() signatures

ArmFeedforward.calculate() requires the current arm angle as its first argument — in radians. ElevatorFeedforward.calculate() does not take a position argument. This is because the elevator's gravity compensation is constant (no geometry), but the arm's gravity compensation depends on where the arm is right now. Every call to ArmFeedforward's calculate must pass a fresh, current angle reading. If you call it with a stale or wrong angle, the gravity compensation is wrong — the arm falls or fights itself.

Measuring kG

kG can be measured manually before running SysId — this is actually a useful sanity check:

Arm kG: Hold the arm horizontal (0°) by hand. Slowly increase the motor's output voltage until the arm just holds its position without drifting when you remove your hand. That voltage is kG. Typical range: 0.3–2.5V depending on arm mass and length.
Elevator kG: With the carriage at mid-height, slowly increase motor output voltage (upward) until the carriage just holds still when released. That voltage is kG. Typical range: 0.5–3V depending on carriage mass.

SysId also measures kG automatically when you characterize an arm or elevator. The quasistatic test's y-intercept contains both kS and kG — the analyzer separates them using the position data logged alongside velocity.

Code Integration — Arm and Elevator Subsystems

Click each tab for the complete subsystem pattern. Click highlighted tokens for explanations.

ArmSubsystem.java / ElevatorSubsystem.java

public class ArmSubsystem extends SubsystemBase {

  private final TalonFX m_motor = new TalonFX(Constants.Arm.kMotorId);

  private final ArmFeedforward m_ff =
    new ArmFeedforward(
      Constants.Arm.kS, // V — static friction
      Constants.Arm.kG, // V — hold at horizontal
      Constants.Arm.kV, // V·s/rad
      Constants.Arm.kA);// V·s²/rad

  private final StatusSignal<Double> m_posSignal =
    m_motor.getPosition();
  private final StatusSignal<Double> m_velSignal =
    m_motor.getVelocity();

  /** Hold a target velocity at the current angle. */
  public void setVelocityRadPS(double targetRadPS) {
    double angleRad = getAngleRadians();
    double ffVolts = m_ff.calculate(angleRad, targetRadPS);
    m_motor.setVoltage(ffVolts);
  }

  /** Returns angle in radians. 0 = horizontal, π/2 = straight up. */
  public double getAngleRadians() {
    return m_posSignal.refresh().getValueAsDouble()
      * 2 * Math.PI // rotations → radians
      / Constants.Arm.kGearRatio;
  }

  /** Zero-velocity hold — gravity compensation only. */
  public void holdPosition() {
    double ffVolts = m_ff.calculate(getAngleRadians(), 0.0);
    m_motor.setVoltage(ffVolts);
  }
}

← click a highlighted token

public class ElevatorSubsystem extends SubsystemBase {

  private final TalonFX m_motor = new TalonFX(Constants.Elevator.kMotorId);

  private final ElevatorFeedforward m_ff =
    new ElevatorFeedforward(
      Constants.Elevator.kS, // V
      Constants.Elevator.kG, // V — hold at rest anywhere
      Constants.Elevator.kV, // V·s/m
      Constants.Elevator.kA);// V·s²/m

  private final StatusSignal<Double> m_velSignal =
    m_motor.getVelocity();

  /** Command a velocity in meters per second. */
  public void setVelocityMPS(double targetMPS) {
    double ffVolts = m_ff.calculate(targetMPS);
    m_motor.setVoltage(ffVolts);
  }

  /** Hold current position — kG provides all needed upward force. */
  public void holdPosition() {
    // No position argument needed — kG is constant for elevator
    double ffVolts = m_ff.calculate(0.0);
    m_motor.setVoltage(ffVolts);
  }

  public double getPositionMeters() {
    return m_motor.getPosition().refresh().getValueAsDouble()
      * Constants.Elevator.kMetersPerRotation;
  }
}

← click a highlighted token

The Gravity-Only Validation Test

Before adding PID to any arm or elevator, run this test to confirm kG is correct. It is the most diagnostic single test available for gravity-affected mechanisms.

💡 The hold test: command velocity = 0 across the range of motion

For an arm: Enable the robot. Call setVelocityRadPS(0) repeatedly from a periodic method. Manually move the arm to 0° (horizontal), 45°, 90°, 135°, and 180° and release it at each position. At every position, the arm should hold still — neither drifting toward nor away from horizontal. If the arm slowly falls at 0°, kG is too small. If it creeps upward at 0°, kG is too large. If it holds at 0° but not at 45°, check that you're passing the correct angle to calculate().

For an elevator: Call holdPosition() from a periodic method. Move the carriage to three different heights and release. It should hold at all three. Since kG is constant, a failure at one height is a failure at all heights — if it doesn't hold anywhere, kG is simply wrong.

🔍 LRI Perspective: "An arm that drifts when the command says 'hold' is always a kG problem"

At inspection, I watch mechanisms during enable. An arm that slowly falls when the code is commanding a setpoint, or an elevator that drifts down during a hold command, always has the same root cause: kG is too small, missing entirely, or not position-corrected. I've seen teams run PID gains so high to compensate for missing gravity feedforward that the arm oscillates violently — and then the programmer increases kD more, and the oscillation gets worse. The fix is never more PID. The fix is always measuring and implementing kG correctly, so PID only needs to handle the last 5% of error.

🔌 System Check — Arm and Elevator Feedforward Bringup

After implementing ArmFeedforward or ElevatorFeedforward:

Run the gravity-only hold test at multiple positions. Confirm no drift. If drift exists, adjust kG and re-test. kG is the single most important constant to get right before proceeding.
Publish getAngleRadians() (arm) or getPositionMeters() (elevator) to SmartDashboard. Manually move the mechanism and confirm the reading changes in the correct direction and with the correct sign convention (0 = horizontal for arm, 0 = bottom for elevator). A wrong sign or wrong unit is the most common ArmFeedforward bug.
Command a slow upward velocity (0.5 rad/s for arm, 0.2 m/s for elevator). Confirm motion is smooth and consistent. Slow downward velocity: same smoothness check. The mechanism should move at approximately the same perceived speed in both directions if kS and kG are correct.
For the arm: specifically test at or near 90° vertical. The kG·cos(90°) = 0 should mean gravity compensation disappears at this point — commanding velocity 0 at exactly 90° should produce a feedforward output of only kS (or zero if velocity is truly zero). Verify this on SmartDashboard by publishing the ffVolts value from inside setVelocityRadPS().

Knowledge Check

1. An arm is at 45° above horizontal (θ = π/4 radians). kG = 1.2V, kV = 0.8 V·s/rad, kS = 0.15V. The arm is commanded to hold still (velocity = 0). What is the feedforward output?

A 1.2V — kG always equals 1.2V at any angle
B 0.15V — only kS applies when velocity is zero
C kG·cos(45°) + 0 = 1.2 × 0.707 ≈ 0.849V. At velocity = 0: kS·sgn(0) = 0 (no motion direction), kV·0 = 0. Only the gravity term contributes.
D 1.2 × cos(45°) + 0.15 = 0.849 + 0.15 = 0.999V — kS always applies

2. You implement an arm subsystem using SimpleMotorFeedforward instead of ArmFeedforward. You set kV and kS correctly from SysId. When you command the arm to hold at 0° (horizontal), what happens?

A The arm holds correctly — kS handles static friction including gravity.
B The arm slowly falls. SimpleMotorFeedforward.calculate(0) returns kS·sgn(0) = 0V (no motion direction, no kS) + kV·0 = 0V total. Zero volts is applied. Gravity is unopposed. The arm falls until PID (if present) corrects it, or until it hits the hard stop.
C The arm oscillates — SimpleMotorFeedforward's kA term interacts with gravity.
D Nothing — WPILib automatically adds gravity compensation to all feedforward classes.

3. Your elevator carriage holds perfectly still at mid-height (kG is correct). After adding game piece weight to the carriage, it now slowly drifts downward at mid-height. Why, and what's the fix?

A The game piece weight changed kV — re-run SysId to get new kV.
B The game piece increased the total mass the elevator must support, so the gravity force increased. kG is now too small — it no longer supplies enough voltage to resist the heavier carriage. Either re-characterize with the game piece loaded (preferred) or manually measure the new kG value with the piece present. For competition, always characterize with the expected loaded mass.
C The game piece changed kS by adding friction to the linear slides.
D ElevatorFeedforward doesn't support position-varying loads — switch to ArmFeedforward.

💪 Practice Prompt

Implement Gravity Compensation on a Real Mechanism

If your robot has an arm: measure kG manually by holding the arm horizontal and slowly increasing voltage until it holds. Start with this hand-measured value. Create an ArmFeedforward field with kS=0.1, your measured kG, kV=0.5, kA=0. Implement holdPosition() exactly as shown above. Run the gravity-only hold test at 0°, 45°, and 90°. Adjust kG until the arm holds at 0° without drifting. Confirm the hold improves as you approach 90° (less kG·cos(θ) needed).
If your robot has an elevator: measure kG by holding the carriage at mid-height and increasing voltage until it holds. Create an ElevatorFeedforward field. Implement holdPosition(). Test at three heights — the hold should be equally effective at all heights. If drift changes with height, check that your position reading isn't affecting the output (it shouldn't — ElevatorFeedforward's calculate() doesn't take position).
Publish the feedforward output voltage from inside your setVelocity/holdPosition method to SmartDashboard as "Arm/FFVolts" or "Elevator/FFVolts". Move the mechanism manually while observing the feedforward output. For the arm, confirm it changes with angle (more voltage near horizontal, less near vertical). For the elevator, confirm it's constant at any height when velocity = 0.
Stretch goal: Add an isLoaded() boolean field and two sets of feedforward constants (empty and loaded). Switch between ArmFeedforward objects based on whether a game piece is detected. Observe how kG automatically changes when a game piece is picked up. This is how competition-level mechanisms handle the mass change of game piece intake/release.