mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-22 01:11:42 +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
@@ -1,152 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <units/angle.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/math.h>
|
||||
#include <units/voltage.h>
|
||||
#include <wpi/MathExtras.h>
|
||||
|
||||
namespace frc {
|
||||
/**
|
||||
* 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).
|
||||
*/
|
||||
class ArmFeedforward {
|
||||
public:
|
||||
using Angle = units::radians;
|
||||
using Velocity = units::radians_per_second;
|
||||
using Acceleration = units::compound_unit<units::radians_per_second,
|
||||
units::inverse<units::second>>;
|
||||
using kv_unit =
|
||||
units::compound_unit<units::volts,
|
||||
units::inverse<units::radians_per_second>>;
|
||||
using ka_unit =
|
||||
units::compound_unit<units::volts, units::inverse<Acceleration>>;
|
||||
|
||||
constexpr ArmFeedforward() = default;
|
||||
|
||||
/**
|
||||
* Creates a new ArmFeedforward with the specified gains.
|
||||
*
|
||||
* @param kS The static gain, in volts.
|
||||
* @param kCos The gravity gain, in volts.
|
||||
* @param kV The velocity gain, in volt seconds per radian.
|
||||
* @param kA The acceleration gain, in volt seconds^2 per radian.
|
||||
*/
|
||||
constexpr ArmFeedforward(
|
||||
units::volt_t kS, units::volt_t kCos, units::unit_t<kv_unit> kV,
|
||||
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
|
||||
: kS(kS), kCos(kCos), kV(kV), kA(kA) {}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and setpoints.
|
||||
*
|
||||
* @param angle The angle setpoint, in radians.
|
||||
* @param velocity The velocity setpoint, in radians per second.
|
||||
* @param acceleration The acceleration setpoint, in radians per second^2.
|
||||
* @return The computed feedforward, in volts.
|
||||
*/
|
||||
units::volt_t Calculate(units::unit_t<Angle> angle,
|
||||
units::unit_t<Velocity> velocity,
|
||||
units::unit_t<Acceleration> acceleration =
|
||||
units::unit_t<Acceleration>(0)) const {
|
||||
return kS * wpi::sgn(velocity) + kCos * units::math::cos(angle) +
|
||||
kV * velocity + kA * acceleration;
|
||||
}
|
||||
|
||||
// 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.
|
||||
*/
|
||||
units::unit_t<Velocity> MaxAchievableVelocity(
|
||||
units::volt_t maxVoltage, units::unit_t<Angle> angle,
|
||||
units::unit_t<Acceleration> acceleration) {
|
||||
// Assume max velocity is positive
|
||||
return (maxVoltage - kS - kCos * units::math::cos(angle) -
|
||||
kA * acceleration) /
|
||||
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.
|
||||
*/
|
||||
units::unit_t<Velocity> MinAchievableVelocity(
|
||||
units::volt_t maxVoltage, units::unit_t<Angle> angle,
|
||||
units::unit_t<Acceleration> acceleration) {
|
||||
// Assume min velocity is negative, ks flips sign
|
||||
return (-maxVoltage + kS - kCos * units::math::cos(angle) -
|
||||
kA * acceleration) /
|
||||
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 and angle.
|
||||
*/
|
||||
units::unit_t<Acceleration> MaxAchievableAcceleration(
|
||||
units::volt_t maxVoltage, units::unit_t<Angle> angle,
|
||||
units::unit_t<Velocity> velocity) {
|
||||
return (maxVoltage - kS * wpi::sgn(velocity) -
|
||||
kCos * units::math::cos(angle) - kV * velocity) /
|
||||
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 and angle.
|
||||
*/
|
||||
units::unit_t<Acceleration> MinAchievableAcceleration(
|
||||
units::volt_t maxVoltage, units::unit_t<Angle> angle,
|
||||
units::unit_t<Velocity> velocity) {
|
||||
return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
|
||||
}
|
||||
|
||||
units::volt_t kS{0};
|
||||
units::volt_t kCos{0};
|
||||
units::unit_t<kv_unit> kV{0};
|
||||
units::unit_t<ka_unit> kA{0};
|
||||
};
|
||||
} // namespace frc
|
||||
@@ -1,131 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <units/voltage.h>
|
||||
#include <wpi/MathExtras.h>
|
||||
|
||||
namespace frc {
|
||||
/**
|
||||
* A helper class that computes feedforward outputs for a simple elevator
|
||||
* (modeled as a motor acting against the force of gravity).
|
||||
*/
|
||||
template <class Distance>
|
||||
class ElevatorFeedforward {
|
||||
public:
|
||||
using Velocity =
|
||||
units::compound_unit<Distance, units::inverse<units::seconds>>;
|
||||
using Acceleration =
|
||||
units::compound_unit<Velocity, units::inverse<units::seconds>>;
|
||||
using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
|
||||
using ka_unit =
|
||||
units::compound_unit<units::volts, units::inverse<Acceleration>>;
|
||||
|
||||
ElevatorFeedforward() = default;
|
||||
|
||||
/**
|
||||
* Creates a new ElevatorFeedforward with the specified gains.
|
||||
*
|
||||
* @param kS The static gain, in volts.
|
||||
* @param kG The gravity gain, in volts.
|
||||
* @param kV The velocity gain, in volt seconds per distance.
|
||||
* @param kA The acceleration gain, in volt seconds^2 per distance.
|
||||
*/
|
||||
constexpr ElevatorFeedforward(
|
||||
units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
|
||||
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
|
||||
: kS(kS), kG(kG), kV(kV), kA(kA) {}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and setpoints.
|
||||
*
|
||||
* @param velocity The velocity setpoint, in distance per second.
|
||||
* @param acceleration The acceleration setpoint, in distance per second^2.
|
||||
* @return The computed feedforward, in volts.
|
||||
*/
|
||||
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
|
||||
units::unit_t<Acceleration> acceleration =
|
||||
units::unit_t<Acceleration>(0)) {
|
||||
return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
|
||||
}
|
||||
|
||||
// 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.
|
||||
*/
|
||||
constexpr units::unit_t<Velocity> MaxAchievableVelocity(
|
||||
units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
|
||||
// Assume max velocity is positive
|
||||
return (maxVoltage - kS - kG - kA * acceleration) / 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.
|
||||
*/
|
||||
constexpr units::unit_t<Velocity> MinAchievableVelocity(
|
||||
units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
|
||||
// Assume min velocity is negative, ks flips sign
|
||||
return (-maxVoltage + kS - kG - kA * acceleration) / 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.
|
||||
*/
|
||||
constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
|
||||
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
|
||||
return (maxVoltage - kS * wpi::sgn(velocity) - kG - kV * velocity) / 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.
|
||||
*/
|
||||
constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
|
||||
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
|
||||
return MaxAchievableAcceleration(-maxVoltage, velocity);
|
||||
}
|
||||
|
||||
units::volt_t kS{0};
|
||||
units::volt_t kG{0};
|
||||
units::unit_t<kv_unit> kV{0};
|
||||
units::unit_t<ka_unit> kA{0};
|
||||
};
|
||||
} // namespace frc
|
||||
@@ -1,132 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <units/time.h>
|
||||
#include <units/voltage.h>
|
||||
#include <wpi/MathExtras.h>
|
||||
|
||||
namespace frc {
|
||||
/**
|
||||
* A helper class that computes feedforward voltages for a simple
|
||||
* permanent-magnet DC motor.
|
||||
*/
|
||||
template <class Distance>
|
||||
class SimpleMotorFeedforward {
|
||||
public:
|
||||
using Velocity =
|
||||
units::compound_unit<Distance, units::inverse<units::seconds>>;
|
||||
using Acceleration =
|
||||
units::compound_unit<Velocity, units::inverse<units::seconds>>;
|
||||
using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
|
||||
using ka_unit =
|
||||
units::compound_unit<units::volts, units::inverse<Acceleration>>;
|
||||
|
||||
constexpr SimpleMotorFeedforward() = default;
|
||||
|
||||
/**
|
||||
* Creates a new SimpleMotorFeedforward with the specified gains.
|
||||
*
|
||||
* @param kS The static gain, in volts.
|
||||
* @param kV The velocity gain, in volt seconds per distance.
|
||||
* @param kA The acceleration gain, in volt seconds^2 per distance.
|
||||
*/
|
||||
constexpr SimpleMotorFeedforward(
|
||||
units::volt_t kS, units::unit_t<kv_unit> kV,
|
||||
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
|
||||
: kS(kS), kV(kV), kA(kA) {}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and setpoints.
|
||||
*
|
||||
* @param velocity The velocity setpoint, in distance per second.
|
||||
* @param acceleration The acceleration setpoint, in distance per second^2.
|
||||
* @return The computed feedforward, in volts.
|
||||
*/
|
||||
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
|
||||
units::unit_t<Acceleration> acceleration =
|
||||
units::unit_t<Acceleration>(0)) const {
|
||||
return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
|
||||
}
|
||||
|
||||
// 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 motor.
|
||||
* @param acceleration The acceleration of the motor.
|
||||
* @return The maximum possible velocity at the given acceleration.
|
||||
*/
|
||||
constexpr units::unit_t<Velocity> MaxAchievableVelocity(
|
||||
units::volt_t maxVoltage,
|
||||
units::unit_t<Acceleration> acceleration) const {
|
||||
// Assume max velocity is positive
|
||||
return (maxVoltage - kS - kA * acceleration) / 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.
|
||||
*/
|
||||
constexpr units::unit_t<Velocity> MinAchievableVelocity(
|
||||
units::volt_t maxVoltage,
|
||||
units::unit_t<Acceleration> acceleration) const {
|
||||
// Assume min velocity is positive, ks flips sign
|
||||
return (-maxVoltage + kS - kA * acceleration) / 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.
|
||||
*/
|
||||
constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
|
||||
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
|
||||
return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / 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 motor.
|
||||
* @param velocity The velocity of the motor.
|
||||
* @return The minimum possible acceleration at the given velocity.
|
||||
*/
|
||||
constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
|
||||
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
|
||||
return MaxAchievableAcceleration(-maxVoltage, velocity);
|
||||
}
|
||||
|
||||
units::volt_t kS{0};
|
||||
units::unit_t<kv_unit> kV{0};
|
||||
units::unit_t<ka_unit> kA{0};
|
||||
};
|
||||
} // namespace frc
|
||||
Reference in New Issue
Block a user