EasyArmFeedForward
Arm Feedforward
An arm feedforward model estimates the output needed to hold or move a rotating arm.
Instead of calculating a correction from position error, it uses the arm's angle, velocity, and acceleration to account for gravity, static friction, and motion.
EasyStack's EasyArmFeedforward calculates this output using four configurable gains. It can be combined with a feedback controller to help an arm follow its target.
Creating an Arm Feedforward
To create an EasyArmFeedforward, provide the gains in the following order:
EasyArmFeedforward feedforward =
new EasyArmFeedforward(kS, kCos, kV, kA);The parameters are:
| Parameter | Description |
|---|---|
| kS | Static friction compensation, applied according to the sign of velocity. |
| kCos | Gravity compensation gain, multiplied by the cosine of the arm angle. |
| kV | Velocity gain. |
| kA | Acceleration gain. |
For example:
EasyArmFeedforward feedforward =
new EasyArmFeedforward(
0.2,
0.8,
1.1,
0.05
);These values are illustrative. Determine the gains for your arm, gearing, and load.
Without Acceleration Compensation
A three-argument constructor is also available:
EasyArmFeedforward feedforward =
new EasyArmFeedforward(kS, kCos, kV);This sets kA to 0.0.
For example:
EasyArmFeedforward feedforward
new EasyArmFeedforward(@.2, 0.8, 1.1);This is equivalent to:
EasyArmFeedforward feedforward
new EasyArmFeedforward(@.2, 0.8, 1.1, 0.0);Units and Output
For a voltage-based model, use the following gain units:
| Gain | Units |
|---|---|
| kS | Volts |
| kCos | Volts |
| kV | Volts per radian per second |
| kA | Volts per radian per second squared |
With these gains, calculate() returns volts.
The class performs arithmetic only. It does not convert voltage to motor power or apply the result to a motor.
Use consistent units throughout the model. The achievable-motion methods also assume that their voltage argument uses the same output units as the gains.
Arm Angle Convention
The gravity term is:
kCos * Math.cos(positionRadians)The angle must be in radians and measured relative to horizontal.
For a positive kCos, the gravity contribution is:
| Arm angle | Radians | Gravity contribution |
|---|---|---|
| Horizontal | 0 | +kCos |
| 45° above horizontal | Math.PI / 4 | Approximately +0.707 * kCos |
| Vertically upward | Math.PI / 2 | Approximately zero |
| Opposite horizontal direction | Math.PI | -kCos |
Configure the angle and motor direction consistently so positive output provides the intended compensation.
If your encoder reports degrees, convert them first:
double angleRadians Math.toRadians(angleDegrees);If the sensor's zero is not horizontal, apply the appropriate offset before calculating feedforward.
Calculating Feedforward
Use calculate() with the arm angle, requested velocity, and requested acceleration:
double voltage = feedforward.calculate(
positionRadians,
velocityRadPerSec,
accelRadPerSecSquared
);The parameters are:
| Parameter | Description |
|---|---|
| positionRadians | Arm angle relative to horizontal, in radians. |
| velocityRadPerSec | Angular velocity, in radians per second. |
| accelRadPerSecSquared | Angular acceleration, in radians per second squared. |
The calculation is:
output kS * Math.signum(velocity)
kCos * Math.cos (position)
velocity
acceleration;For example:
double voltage = feedforward.calculate(
Math.toRadians(30.0),
1.0,
0.5
);This calculates the modeled output at:
An angle of 30°.
A velocity of 1.0 rad/s.
An acceleration of 0.5 rad/s².
For motion tracking, velocity and acceleration normally come from the desired trajectory. The angle can come from that reference or from the measured arm angle, depending on how the control system applies gravity compensation.
Calculating Without Acceleration
Use the two-argument overload when acceleration compensation is not needed:
double voltage = feedforward.calculate(
positionRadians,
velocityRadPerSec
);For example:
double voltage = feedforward.calculate(
Math.toRadians(30.0),
1.0
);This is equivalent to:
double voltage = feedforward.calculate(
Math.toRadians(30.0),
1.0,
0.0
);The acceleration contribution is zero even if the object has a nonzero kA.
Holding an Arm Still
For a stationary arm, provide zero velocity and acceleration:
double holdingVoltage = feedforward.calculate(
currentAngleRadians,
0.0,
0.0
);The result becomes:
holdingVoltage kCos * Math.cos(currentAngleRadians);For example, with kCos = 0.8 and an angle of 60°:
holdingVoltage = 0.8 * Math.cos(Math.toRadians(60.0));The estimated holding output is 0.4 volts.
This compensates for the modeled gravity load. It does not correct an angle error by itself.
Static Friction Compensation
The static term follows the sign of the supplied velocity:
staticCompensation = kS * Math.signum(velocity);Its behavior is:
| Velocity | Static contribution |
|---|---|
| Positive | +kS |
| Negative | -kS |
| Zero | 0.0 |
At exactly zero velocity, the static contribution is zero, even if the requested acceleration is nonzero.
The implementation does not use acceleration or position error to choose a starting direction for static compensation.
Combining Feedforward with PID
Feedforward provides a modeled output, while PID corrects the difference between the desired and measured position.
Add their outputs when both use the same output units:
double pidVoltage = controller.calculate(
measuredAngleRadians,
targetAngleRadians
);
double feedforwardVoltage = feedforward.calculate(
measuredAngleRadians,
targetVelocityRadPerSec,
targetAccelerationRadPerSecSquared
);
double requestedVoltage = pidVoltage + feedforwardVoltage;In this example:
PID uses radians for position and produces volts.
Gravity compensation uses the measured angle.
Velocity and acceleration compensation use the desired motion.
For a fixed position target:
double feedforwardVoltage = feedforward.calculate(
measuredAngleRadians,
0.0
);The PID gains must be tuned to produce volts if the feedforward gains are voltage-based. Do not directly add a motor-power output to a voltage output.
Maximum Achievable Velocity
Use maxAchievableVelocity() to calculate a velocity boundary for a given voltage, angle, and acceleration:
double maxVelocity = feedforward.maxAchievableVelocity(
maxVoltage,
angleRadians,
accelerationRadPerSecSquared
);The parameters are:
| Parameter | Description |
|---|---|
| maxVoltage | Positive available voltage magnitude. |
| angle | Arm angle in radians. |
| acceleration | Angular acceleration in rad/s². |
For example:
double maxVelocity = feedforward.maxAchievableVelocity(
12.0,
Math.toRadians(30.0),
0.0
);The calculation is:
maxVelocity =
(maxVoltage
- kS
- kCos * Math.cos(angle)
- kA * acceleration)
/ kV;The result is in radians per second when using the voltage-based units described above.
This formula assumes the positive-velocity branch of static friction compensation. A negative result should not be interpreted as a validated reverse-motion limit.
Minimum Achievable Velocity
Use minAchievableVelocity() for the negative-voltage boundary:
double minVelocity = feedforward.minAchievableVelocity(
maxVoltage,
angleRadians,
accelerationRadPerSecSquared
);For example:
double minVelocity = feedforward.minAchievableVelocity(
12.0,
Math.toRadians(30.0),
0.0
);The calculation is:
minVelocity =
(-maxVoltage
+ kS
- kCos * Math.cos(angle)
- kA * acceleration)
/ kV;This formula assumes the negative-velocity branch.
Both velocity methods require a nonzero kV. For the usual interpretation of maximum and minimum boundaries, use a positive kV.
The results are model estimates, not enforced motion limits.
Maximum Achievable Acceleration
Use maxAchievableAcceleration():
double maxAcceleration =
feedforward.maxAchievableAcceleration(
maxVoltage,
angleRadians,
velocityRadPerSec
);For example:
double maxAcceleration =
feedforward.maxAchievableAcceleration(
12.0,
Math.toRadians(30.0),
1.0
);The calculation is:
maxAcceleration =
(maxVoltage
- kS * Math.signum(velocity)
- kCos * Math.cos(angle)
- kV * velocity)
/ kA;The result is in radians per second squared.
Unlike the velocity-boundary methods, this method chooses the static term from the supplied velocity's sign.
Minimum Achievable Acceleration
Use minAchievableAcceleration():
double minAcceleration =
feedforward.minAchievableAcceleration(
maxVoltage,
angleRadians,
velocityRadPerSec
);Internally, it evaluates the acceleration formula at negative maximum voltage:
return maxAchievableAcceleration(
-maxVoltage,
angle,
velocity
);Both acceleration methods require a nonzero kA. For the usual maximum and minimum interpretation, use a positive kA.
The three-argument constructor sets kA to zero, so its acceleration-boundary calculations are not usable. Division by zero can return infinity or NaN.
Reading the Gains
The gains are public and final:
= feedforward.kS;
= feedforward.kCos;
= feedforward.kV;
= feedforward.kA;They can be read directly but cannot be reassigned after construction.
To change the gains, create another instance:
feedforward
new EasyArmFeedforward(0.25, 0.9, 1.2, 0.06);The class does not validate the supplied gains.
Output Limits
calculate() does not clamp its result to the available voltage.
Apply any required limit before commanding the motor:
double requestedVoltage = feedforward.calculate(
angleRadians,
velocityRadPerSec,
accelerationRadPerSecSquared
);
double maxVoltage = 12.0;
double appliedVoltage = Math.max(
-maxVoltage,
Math.min(maxVoltage, requestedVoltage)
);For an FTC motor controlled through setPower(), convert voltage to a normalized power command using a valid measured supply voltage:
double power = appliedVoltage / batteryVoltage;
power = Math.max(-1.0, Math.min(1.0, power));
motor.setPower(power);This is an approximate voltage command through motor power. The final clamp prevents requesting power outside -1.0 to 1.0.
Complete Example
The following control-loop example combines the previously shown EasyPIDController with arm feedforward to hold a target angle.
It assumes your sensor conversion already provides the arm's angle in radians relative to horizontal.
Create the controllers before entering the loop:
import dev.easyfamily.easystack.control.EasyPIDController;
import dev.easyfamily.easystack.control.wpiLibControllers.EasyArmFeedforward;
// Illustrative gains: tune for your arm.
// PID input: radians. PID output: volts.
EasyPIDController controller =
new EasyPIDController(3.0, 0.0, 0.1);
EasyArmFeedforward feedforward =
new EasyArmFeedforward(
0.2,
0.8,
1.1,
0.05
);
controller.setTolerance(Math.toRadians(2.0));
double targetAngleRadians = Math.toRadians(45.0);controller.setTargetPosition(targetAngleRadians);Inside the active control loop, obtain fresh values for measuredAngleRadians and batteryVoltage, then calculate:
double pidVoltage = controller.calculate(
measuredAngleRadians
);
double feedforwardVoltage = feedforward.calculate(
measuredAngleRadians,
0.0
);
double requestedVoltage = pidVoltage + feedforwardVoltage;
double power = 0.0;
if (Double.isFinite(batteryVoltage) && batteryVoltage > 0.0) {
double voltageLimit = Math.min(12.0, batteryVoltage);
double appliedVoltage = Math.max(
-voltageLimit,
Math.min(voltageLimit, requestedVoltage)
); power = appliedVoltage / batteryVoltage;
}
motor.setPower(power);
telemetry.addData(
"Angle (deg)",
Math.toDegrees(measuredAngleRadians)
);
telemetry.addData(
"Target (deg)",
Math.toDegrees(targetAngleRadians)
);
telemetry.addData("PID Voltage", pidVoltage);
telemetry.addData("Feedforward Voltage", feedforwardVoltage);
telemetry.addData("At Target", controller.atTarget());
telemetry.addData("Power", power);
telemetry.update();Configure the FTC motor for direct power control when this loop controls the mechanism.
For a moving reference, replace the fixed target and zero motion values with your reference position, velocity, and acceleration. EasyArmFeedforward does not generate that reference internally.
When control ends, command the motor separately:
motor.setPower(0.0);
controller.reset();EasyArmFeedforward has no accumulated state and does not require a reset.
Last updated 2026-11-16