mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-28 02:11:43 +00:00
[wpimath] Move math functionality into new wpimath library (#2629)
The wpimath library is a new library designed to separate the reusable math functionality from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j). Package names / include file names were NOT changed to minimize breakage. In a future year it would be good to revamp these for a more uniform user experience and to reduce the risk of accidental naming conflicts. While theoretically all of this functionality could be placed into wpiutil, several pieces of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this expense for users who only want cscore or ntcore. It also allows for easy future separation of build tasks vs number of workers on memory-constrained machines. This moves the following functionality from wpiutil into wpimath: - Eigen - ejml - Drake - DARE - wpiutil.math package (Matrix etc) - units And the following functionality from wpilibc/j into wpimath: - Geometry - Kinematics - Spline - Trajectory - LinearFilter - MedianFilter - Feed-forward controllers
This commit is contained in:
Peter Johnson
@@ -0,0 +1,144 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.controller;
|
||||
|
||||
/**
|
||||
* A helper class that computes feedforward outputs for a simple arm (modeled as a motor
|
||||
* acting against the force of gravity on a beam suspended at an angle).
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
public class ArmFeedforward {
|
||||
public final double ks;
|
||||
public final double kcos;
|
||||
public final double kv;
|
||||
public final double ka;
|
||||
|
||||
/**
|
||||
* Creates a new ArmFeedforward with the specified gains. Units of the gain values
|
||||
* will dictate units of the computed feedforward.
|
||||
*
|
||||
* @param ks The static gain.
|
||||
* @param kcos The gravity gain.
|
||||
* @param kv The velocity gain.
|
||||
* @param ka The acceleration gain.
|
||||
*/
|
||||
public ArmFeedforward(double ks, double kcos, double kv, double ka) {
|
||||
this.ks = ks;
|
||||
this.kcos = kcos;
|
||||
this.kv = kv;
|
||||
this.ka = ka;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new ArmFeedforward with the specified gains. Acceleration gain is
|
||||
* defaulted to zero. Units of the gain values will dictate units of the computed feedforward.
|
||||
*
|
||||
* @param ks The static gain.
|
||||
* @param kcos The gravity gain.
|
||||
* @param kv The velocity gain.
|
||||
*/
|
||||
public ArmFeedforward(double ks, double kcos, double kv) {
|
||||
this(ks, kcos, kv, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and setpoints.
|
||||
*
|
||||
* @param positionRadians The position (angle) setpoint.
|
||||
* @param velocityRadPerSec The velocity setpoint.
|
||||
* @param accelRadPerSecSquared The acceleration setpoint.
|
||||
* @return The computed feedforward.
|
||||
*/
|
||||
public double calculate(double positionRadians, double velocityRadPerSec,
|
||||
double accelRadPerSecSquared) {
|
||||
return ks * Math.signum(velocityRadPerSec) + kcos * Math.cos(positionRadians)
|
||||
+ kv * velocityRadPerSec
|
||||
+ ka * accelRadPerSecSquared;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
|
||||
* be zero).
|
||||
*
|
||||
* @param positionRadians The position (angle) setpoint.
|
||||
* @param velocity The velocity setpoint.
|
||||
* @return The computed feedforward.
|
||||
*/
|
||||
public double calculate(double positionRadians, double velocity) {
|
||||
return calculate(positionRadians, velocity, 0);
|
||||
}
|
||||
|
||||
// Rearranging the main equation from the calculate() method yields the
|
||||
// formulas for the methods below:
|
||||
|
||||
/**
|
||||
* Calculates the maximum achievable velocity given a maximum voltage supply,
|
||||
* a position, and an acceleration. Useful for ensuring that velocity and
|
||||
* acceleration constraints for a trapezoidal profile are simultaneously
|
||||
* achievable - enter the acceleration constraint, and this will give you
|
||||
* a simultaneously-achievable velocity constraint.
|
||||
*
|
||||
* @param maxVoltage The maximum voltage that can be supplied to the arm.
|
||||
* @param angle The angle of the arm.
|
||||
* @param acceleration The acceleration of the arm.
|
||||
* @return The maximum possible velocity at the given acceleration and angle.
|
||||
*/
|
||||
public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
|
||||
// Assume max velocity is positive
|
||||
return (maxVoltage - ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the minimum achievable velocity given a maximum voltage supply,
|
||||
* a position, and an acceleration. Useful for ensuring that velocity and
|
||||
* acceleration constraints for a trapezoidal profile are simultaneously
|
||||
* achievable - enter the acceleration constraint, and this will give you
|
||||
* a simultaneously-achievable velocity constraint.
|
||||
*
|
||||
* @param maxVoltage The maximum voltage that can be supplied to the arm.
|
||||
* @param angle The angle of the arm.
|
||||
* @param acceleration The acceleration of the arm.
|
||||
* @return The minimum possible velocity at the given acceleration and angle.
|
||||
*/
|
||||
public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
|
||||
// Assume min velocity is negative, ks flips sign
|
||||
return (-maxVoltage + ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the maximum achievable acceleration given a maximum voltage
|
||||
* supply, a position, and a velocity. Useful for ensuring that velocity and
|
||||
* acceleration constraints for a trapezoidal profile are simultaneously
|
||||
* achievable - enter the velocity constraint, and this will give you
|
||||
* a simultaneously-achievable acceleration constraint.
|
||||
*
|
||||
* @param maxVoltage The maximum voltage that can be supplied to the arm.
|
||||
* @param angle The angle of the arm.
|
||||
* @param velocity The velocity of the arm.
|
||||
* @return The maximum possible acceleration at the given velocity.
|
||||
*/
|
||||
public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
|
||||
return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kcos - velocity * kv) / ka;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the minimum achievable acceleration given a maximum voltage
|
||||
* supply, a position, and a velocity. Useful for ensuring that velocity and
|
||||
* acceleration constraints for a trapezoidal profile are simultaneously
|
||||
* achievable - enter the velocity constraint, and this will give you
|
||||
* a simultaneously-achievable acceleration constraint.
|
||||
*
|
||||
* @param maxVoltage The maximum voltage that can be supplied to the arm.
|
||||
* @param angle The angle of the arm.
|
||||
* @param velocity The velocity of the arm.
|
||||
* @return The minimum possible acceleration at the given velocity.
|
||||
*/
|
||||
public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
|
||||
return maxAchievableAcceleration(-maxVoltage, angle, velocity);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,135 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.controller;
|
||||
|
||||
/**
|
||||
* A helper class that computes feedforward outputs for a simple elevator (modeled as a motor
|
||||
* acting against the force of gravity).
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
public class ElevatorFeedforward {
|
||||
public final double ks;
|
||||
public final double kg;
|
||||
public final double kv;
|
||||
public final double ka;
|
||||
|
||||
/**
|
||||
* Creates a new ElevatorFeedforward with the specified gains. Units of the gain values
|
||||
* will dictate units of the computed feedforward.
|
||||
*
|
||||
* @param ks The static gain.
|
||||
* @param kg The gravity gain.
|
||||
* @param kv The velocity gain.
|
||||
* @param ka The acceleration gain.
|
||||
*/
|
||||
public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
|
||||
this.ks = ks;
|
||||
this.kg = kg;
|
||||
this.kv = kv;
|
||||
this.ka = ka;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is
|
||||
* defaulted to zero. Units of the gain values will dictate units of the computed feedforward.
|
||||
*
|
||||
* @param ks The static gain.
|
||||
* @param kg The gravity gain.
|
||||
* @param kv The velocity gain.
|
||||
*/
|
||||
public ElevatorFeedforward(double ks, double kg, double kv) {
|
||||
this(ks, kg, kv, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and setpoints.
|
||||
*
|
||||
* @param velocity The velocity setpoint.
|
||||
* @param acceleration The acceleration setpoint.
|
||||
* @return The computed feedforward.
|
||||
*/
|
||||
public double calculate(double velocity, double acceleration) {
|
||||
return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
|
||||
* be zero).
|
||||
*
|
||||
* @param velocity The velocity setpoint.
|
||||
* @return The computed feedforward.
|
||||
*/
|
||||
public double calculate(double velocity) {
|
||||
return calculate(velocity, 0);
|
||||
}
|
||||
|
||||
// Rearranging the main equation from the calculate() method yields the
|
||||
// formulas for the methods below:
|
||||
|
||||
/**
|
||||
* Calculates the maximum achievable velocity given a maximum voltage supply
|
||||
* and an acceleration. Useful for ensuring that velocity and
|
||||
* acceleration constraints for a trapezoidal profile are simultaneously
|
||||
* achievable - enter the acceleration constraint, and this will give you
|
||||
* a simultaneously-achievable velocity constraint.
|
||||
*
|
||||
* @param maxVoltage The maximum voltage that can be supplied to the elevator.
|
||||
* @param acceleration The acceleration of the elevator.
|
||||
* @return The maximum possible velocity at the given acceleration.
|
||||
*/
|
||||
public double maxAchievableVelocity(double maxVoltage, double acceleration) {
|
||||
// Assume max velocity is positive
|
||||
return (maxVoltage - ks - kg - acceleration * ka) / kv;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the minimum achievable velocity given a maximum voltage supply
|
||||
* and an acceleration. Useful for ensuring that velocity and
|
||||
* acceleration constraints for a trapezoidal profile are simultaneously
|
||||
* achievable - enter the acceleration constraint, and this will give you
|
||||
* a simultaneously-achievable velocity constraint.
|
||||
*
|
||||
* @param maxVoltage The maximum voltage that can be supplied to the elevator.
|
||||
* @param acceleration The acceleration of the elevator.
|
||||
* @return The minimum possible velocity at the given acceleration.
|
||||
*/
|
||||
public double minAchievableVelocity(double maxVoltage, double acceleration) {
|
||||
// Assume min velocity is negative, ks flips sign
|
||||
return (-maxVoltage + ks - kg - acceleration * ka) / kv;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the maximum achievable acceleration given a maximum voltage
|
||||
* supply and a velocity. Useful for ensuring that velocity and
|
||||
* acceleration constraints for a trapezoidal profile are simultaneously
|
||||
* achievable - enter the velocity constraint, and this will give you
|
||||
* a simultaneously-achievable acceleration constraint.
|
||||
*
|
||||
* @param maxVoltage The maximum voltage that can be supplied to the elevator.
|
||||
* @param velocity The velocity of the elevator.
|
||||
* @return The maximum possible acceleration at the given velocity.
|
||||
*/
|
||||
public double maxAchievableAcceleration(double maxVoltage, double velocity) {
|
||||
return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the minimum achievable acceleration given a maximum voltage
|
||||
* supply and a velocity. Useful for ensuring that velocity and
|
||||
* acceleration constraints for a trapezoidal profile are simultaneously
|
||||
* achievable - enter the velocity constraint, and this will give you
|
||||
* a simultaneously-achievable acceleration constraint.
|
||||
*
|
||||
* @param maxVoltage The maximum voltage that can be supplied to the elevator.
|
||||
* @param velocity The velocity of the elevator.
|
||||
* @return The minimum possible acceleration at the given velocity.
|
||||
*/
|
||||
public double minAchievableAcceleration(double maxVoltage, double velocity) {
|
||||
return maxAchievableAcceleration(-maxVoltage, velocity);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,130 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.controller;
|
||||
|
||||
/**
|
||||
* A helper class that computes feedforward outputs for a simple permanent-magnet DC motor.
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
public class SimpleMotorFeedforward {
|
||||
public final double ks;
|
||||
public final double kv;
|
||||
public final double ka;
|
||||
|
||||
/**
|
||||
* Creates a new SimpleMotorFeedforward with the specified gains. Units of the gain values
|
||||
* will dictate units of the computed feedforward.
|
||||
*
|
||||
* @param ks The static gain.
|
||||
* @param kv The velocity gain.
|
||||
* @param ka The acceleration gain.
|
||||
*/
|
||||
public SimpleMotorFeedforward(double ks, double kv, double ka) {
|
||||
this.ks = ks;
|
||||
this.kv = kv;
|
||||
this.ka = ka;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is
|
||||
* defaulted to zero. Units of the gain values will dictate units of the computed feedforward.
|
||||
*
|
||||
* @param ks The static gain.
|
||||
* @param kv The velocity gain.
|
||||
*/
|
||||
public SimpleMotorFeedforward(double ks, double kv) {
|
||||
this(ks, kv, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and setpoints.
|
||||
*
|
||||
* @param velocity The velocity setpoint.
|
||||
* @param acceleration The acceleration setpoint.
|
||||
* @return The computed feedforward.
|
||||
*/
|
||||
public double calculate(double velocity, double acceleration) {
|
||||
return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
|
||||
}
|
||||
|
||||
// Rearranging the main equation from the calculate() method yields the
|
||||
// formulas for the methods below:
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
|
||||
* be zero).
|
||||
*
|
||||
* @param velocity The velocity setpoint.
|
||||
* @return The computed feedforward.
|
||||
*/
|
||||
public double calculate(double velocity) {
|
||||
return calculate(velocity, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the maximum achievable velocity given a maximum voltage supply
|
||||
* and an acceleration. Useful for ensuring that velocity and
|
||||
* acceleration constraints for a trapezoidal profile are simultaneously
|
||||
* achievable - enter the acceleration constraint, and this will give you
|
||||
* a simultaneously-achievable velocity constraint.
|
||||
*
|
||||
* @param maxVoltage The maximum voltage that can be supplied to the motor.
|
||||
* @param acceleration The acceleration of the motor.
|
||||
* @return The maximum possible velocity at the given acceleration.
|
||||
*/
|
||||
public double maxAchievableVelocity(double maxVoltage, double acceleration) {
|
||||
// Assume max velocity is positive
|
||||
return (maxVoltage - ks - acceleration * ka) / kv;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the minimum achievable velocity given a maximum voltage supply
|
||||
* and an acceleration. Useful for ensuring that velocity and
|
||||
* acceleration constraints for a trapezoidal profile are simultaneously
|
||||
* achievable - enter the acceleration constraint, and this will give you
|
||||
* a simultaneously-achievable velocity constraint.
|
||||
*
|
||||
* @param maxVoltage The maximum voltage that can be supplied to the motor.
|
||||
* @param acceleration The acceleration of the motor.
|
||||
* @return The minimum possible velocity at the given acceleration.
|
||||
*/
|
||||
public double minAchievableVelocity(double maxVoltage, double acceleration) {
|
||||
// Assume min velocity is negative, ks flips sign
|
||||
return (-maxVoltage + ks - acceleration * ka) / kv;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the maximum achievable acceleration given a maximum voltage
|
||||
* supply and a velocity. Useful for ensuring that velocity and
|
||||
* acceleration constraints for a trapezoidal profile are simultaneously
|
||||
* achievable - enter the velocity constraint, and this will give you
|
||||
* a simultaneously-achievable acceleration constraint.
|
||||
*
|
||||
* @param maxVoltage The maximum voltage that can be supplied to the motor.
|
||||
* @param velocity The velocity of the motor.
|
||||
* @return The maximum possible acceleration at the given velocity.
|
||||
*/
|
||||
public double maxAchievableAcceleration(double maxVoltage, double velocity) {
|
||||
return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the maximum achievable acceleration given a maximum voltage
|
||||
* supply and a velocity. Useful for ensuring that velocity and
|
||||
* acceleration constraints for a trapezoidal profile are simultaneously
|
||||
* achievable - enter the velocity constraint, and this will give you
|
||||
* a simultaneously-achievable acceleration constraint.
|
||||
*
|
||||
* @param maxVoltage The maximum voltage that can be supplied to the motor.
|
||||
* @param velocity The velocity of the motor.
|
||||
* @return The minimum possible acceleration at the given velocity.
|
||||
*/
|
||||
public double minAchievableAcceleration(double maxVoltage, double velocity) {
|
||||
return maxAchievableAcceleration(-maxVoltage, velocity);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user