mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpimath] Move Java classes to edu.wpi.first.math (#3316)
This commit is contained in:
Noam Zaks
@@ -0,0 +1,138 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.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,193 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Nat;
|
||||
import edu.wpi.first.math.Num;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.system.NumericalJacobian;
|
||||
import java.util.function.BiFunction;
|
||||
import java.util.function.Function;
|
||||
|
||||
/**
|
||||
* Constructs a control-affine plant inversion model-based feedforward from given model dynamics.
|
||||
*
|
||||
* <p>If given the vector valued function as f(x, u) where x is the state vector and u is the input
|
||||
* vector, the B matrix(continuous input matrix) is calculated through a {@link
|
||||
* edu.wpi.first.math.system.NumericalJacobian}. In this case f has to be control-affine (of the
|
||||
* form f(x) + Bu).
|
||||
*
|
||||
* <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (rDot - f(x))</strong>, where
|
||||
* <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
|
||||
*
|
||||
* <p>This feedforward does not account for a dynamic B matrix, B is either determined or supplied
|
||||
* when the feedforward is created and remains constant.
|
||||
*
|
||||
* <p>For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
|
||||
public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> {
|
||||
/** The current reference state. */
|
||||
@SuppressWarnings("MemberName")
|
||||
private Matrix<States, N1> m_r;
|
||||
|
||||
/** The computed feedforward. */
|
||||
private Matrix<Inputs, N1> m_uff;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
private final Matrix<States, Inputs> m_B;
|
||||
|
||||
private final Nat<Inputs> m_inputs;
|
||||
|
||||
private final double m_dt;
|
||||
|
||||
/** The model dynamics. */
|
||||
private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
|
||||
|
||||
/**
|
||||
* Constructs a feedforward with given model dynamics as a function of state and input.
|
||||
*
|
||||
* @param states A {@link Nat} representing the number of states.
|
||||
* @param inputs A {@link Nat} representing the number of inputs.
|
||||
* @param f A vector-valued function of x, the state, and u, the input, that returns the
|
||||
* derivative of the state vector. HAS to be control-affine (of the form f(x) + Bu).
|
||||
* @param dtSeconds The timestep between calls of calculate().
|
||||
*/
|
||||
public ControlAffinePlantInversionFeedforward(
|
||||
Nat<States> states,
|
||||
Nat<Inputs> inputs,
|
||||
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
|
||||
double dtSeconds) {
|
||||
this.m_dt = dtSeconds;
|
||||
this.m_f = f;
|
||||
this.m_inputs = inputs;
|
||||
|
||||
this.m_B =
|
||||
NumericalJacobian.numericalJacobianU(
|
||||
states, inputs, m_f, new Matrix<>(states, Nat.N1()), new Matrix<>(inputs, Nat.N1()));
|
||||
|
||||
m_r = new Matrix<>(states, Nat.N1());
|
||||
m_uff = new Matrix<>(inputs, Nat.N1());
|
||||
|
||||
reset(m_r);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a feedforward with given model dynamics as a function of state, and the plant's
|
||||
* B(continuous input matrix) matrix.
|
||||
*
|
||||
* @param states A {@link Nat} representing the number of states.
|
||||
* @param inputs A {@link Nat} representing the number of inputs.
|
||||
* @param f A vector-valued function of x, the state, that returns the derivative of the state
|
||||
* vector.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param dtSeconds The timestep between calls of calculate().
|
||||
*/
|
||||
public ControlAffinePlantInversionFeedforward(
|
||||
Nat<States> states,
|
||||
Nat<Inputs> inputs,
|
||||
Function<Matrix<States, N1>, Matrix<States, N1>> f,
|
||||
Matrix<States, Inputs> B,
|
||||
double dtSeconds) {
|
||||
this.m_dt = dtSeconds;
|
||||
this.m_inputs = inputs;
|
||||
|
||||
this.m_f = (x, u) -> f.apply(x);
|
||||
this.m_B = B;
|
||||
|
||||
m_r = new Matrix<>(states, Nat.N1());
|
||||
m_uff = new Matrix<>(inputs, Nat.N1());
|
||||
|
||||
reset(m_r);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the previously calculated feedforward as an input vector.
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
public Matrix<Inputs, N1> getUff() {
|
||||
return m_uff;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the previously calculated feedforward.
|
||||
*
|
||||
* @param row Row of uff.
|
||||
* @return The row of the calculated feedforward.
|
||||
*/
|
||||
public double getUff(int row) {
|
||||
return m_uff.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current reference vector r.
|
||||
*
|
||||
* @return The current reference vector.
|
||||
*/
|
||||
public Matrix<States, N1> getR() {
|
||||
return m_r;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the current reference vector r.
|
||||
*
|
||||
* @param row Row of r.
|
||||
* @return The row of the current reference vector.
|
||||
*/
|
||||
public double getR(int row) {
|
||||
return m_r.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a specified initial state vector.
|
||||
*
|
||||
* @param initialState The initial state vector.
|
||||
*/
|
||||
public void reset(Matrix<States, N1> initialState) {
|
||||
m_r = initialState;
|
||||
m_uff.fill(0.0);
|
||||
}
|
||||
|
||||
/** Resets the feedforward with a zero initial state vector. */
|
||||
public void reset() {
|
||||
m_r.fill(0.0);
|
||||
m_uff.fill(0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with only the desired future reference. This uses the internally
|
||||
* stored "current" reference.
|
||||
*
|
||||
* <p>If this method is used the initial state of the system is the one set using {@link
|
||||
* LinearPlantInversionFeedforward#reset(Matrix)}. If the initial state is not set it defaults to
|
||||
* a zero vector.
|
||||
*
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
|
||||
return calculate(m_r, nextR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with current and future reference vectors.
|
||||
*
|
||||
* @param r The reference state of the current timestep (k).
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
|
||||
var rDot = (nextR.minus(r)).div(m_dt);
|
||||
|
||||
m_uff = m_B.solve(rDot.minus(m_f.apply(r, new Matrix<>(m_inputs, Nat.N1()))));
|
||||
|
||||
m_r = nextR;
|
||||
return m_uff;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,128 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.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,153 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Num;
|
||||
import edu.wpi.first.math.math.Discretization;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.system.LinearSystem;
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
|
||||
/**
|
||||
* Constructs a plant inversion model-based feedforward from a {@link LinearSystem}.
|
||||
*
|
||||
* <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A r_k) </strong>,
|
||||
* where <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
|
||||
*
|
||||
* <p>For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
|
||||
public class LinearPlantInversionFeedforward<
|
||||
States extends Num, Inputs extends Num, Outputs extends Num> {
|
||||
/** The current reference state. */
|
||||
@SuppressWarnings("MemberName")
|
||||
private Matrix<States, N1> m_r;
|
||||
|
||||
/** The computed feedforward. */
|
||||
private Matrix<Inputs, N1> m_uff;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
private final Matrix<States, Inputs> m_B;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
private final Matrix<States, States> m_A;
|
||||
|
||||
/**
|
||||
* Constructs a feedforward with the given plant.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
public LinearPlantInversionFeedforward(
|
||||
LinearSystem<States, Inputs, Outputs> plant, double dtSeconds) {
|
||||
this(plant.getA(), plant.getB(), dtSeconds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a feedforward with the given coefficients.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public LinearPlantInversionFeedforward(
|
||||
Matrix<States, States> A, Matrix<States, Inputs> B, double dtSeconds) {
|
||||
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
|
||||
this.m_A = discABPair.getFirst();
|
||||
this.m_B = discABPair.getSecond();
|
||||
|
||||
m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
|
||||
m_uff = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
|
||||
|
||||
reset(m_r);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the previously calculated feedforward as an input vector.
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
public Matrix<Inputs, N1> getUff() {
|
||||
return m_uff;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the previously calculated feedforward.
|
||||
*
|
||||
* @param row Row of uff.
|
||||
* @return The row of the calculated feedforward.
|
||||
*/
|
||||
public double getUff(int row) {
|
||||
return m_uff.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current reference vector r.
|
||||
*
|
||||
* @return The current reference vector.
|
||||
*/
|
||||
public Matrix<States, N1> getR() {
|
||||
return m_r;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the current reference vector r.
|
||||
*
|
||||
* @param row Row of r.
|
||||
* @return The row of the current reference vector.
|
||||
*/
|
||||
public double getR(int row) {
|
||||
return m_r.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a specified initial state vector.
|
||||
*
|
||||
* @param initialState The initial state vector.
|
||||
*/
|
||||
public void reset(Matrix<States, N1> initialState) {
|
||||
m_r = initialState;
|
||||
m_uff.fill(0.0);
|
||||
}
|
||||
|
||||
/** Resets the feedforward with a zero initial state vector. */
|
||||
public void reset() {
|
||||
m_r.fill(0.0);
|
||||
m_uff.fill(0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with only the desired future reference. This uses the internally
|
||||
* stored "current" reference.
|
||||
*
|
||||
* <p>If this method is used the initial state of the system is the one set using {@link
|
||||
* LinearPlantInversionFeedforward#reset(Matrix)}. If the initial state is not set it defaults to
|
||||
* a zero vector.
|
||||
*
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
|
||||
return calculate(m_r, nextR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with current and future reference vectors.
|
||||
*
|
||||
* @param r The reference state of the current timestep (k).
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
|
||||
m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
|
||||
|
||||
m_r = nextR;
|
||||
return m_uff;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,232 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import edu.wpi.first.math.Drake;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Nat;
|
||||
import edu.wpi.first.math.Num;
|
||||
import edu.wpi.first.math.Vector;
|
||||
import edu.wpi.first.math.math.Discretization;
|
||||
import edu.wpi.first.math.math.StateSpaceUtil;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.system.LinearSystem;
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
|
||||
/**
|
||||
* Contains the controller coefficients and logic for a linear-quadratic regulator (LQR). LQRs use
|
||||
* the control law u = K(r - x).
|
||||
*
|
||||
* <p>For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
@SuppressWarnings("ClassTypeParameterName")
|
||||
public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Outputs extends Num> {
|
||||
/** The current reference state. */
|
||||
@SuppressWarnings("MemberName")
|
||||
private Matrix<States, N1> m_r;
|
||||
|
||||
/** The computed and capped controller output. */
|
||||
@SuppressWarnings("MemberName")
|
||||
private Matrix<Inputs, N1> m_u;
|
||||
|
||||
// Controller gain.
|
||||
@SuppressWarnings("MemberName")
|
||||
private Matrix<Inputs, States> m_K;
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param qelms The maximum desired error tolerance for each state.
|
||||
* @param relms The maximum desired control effort for each input.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
public LinearQuadraticRegulator(
|
||||
LinearSystem<States, Inputs, Outputs> plant,
|
||||
Vector<States> qelms,
|
||||
Vector<Inputs> relms,
|
||||
double dtSeconds) {
|
||||
this(
|
||||
plant.getA(),
|
||||
plant.getB(),
|
||||
StateSpaceUtil.makeCostMatrix(qelms),
|
||||
StateSpaceUtil.makeCostMatrix(relms),
|
||||
dtSeconds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param qelms The maximum desired error tolerance for each state.
|
||||
* @param relms The maximum desired control effort for each input.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public LinearQuadraticRegulator(
|
||||
Matrix<States, States> A,
|
||||
Matrix<States, Inputs> B,
|
||||
Vector<States> qelms,
|
||||
Vector<Inputs> relms,
|
||||
double dtSeconds) {
|
||||
this(
|
||||
A,
|
||||
B,
|
||||
StateSpaceUtil.makeCostMatrix(qelms),
|
||||
StateSpaceUtil.makeCostMatrix(relms),
|
||||
dtSeconds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Q The state cost matrix.
|
||||
* @param R The input cost matrix.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public LinearQuadraticRegulator(
|
||||
Matrix<States, States> A,
|
||||
Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R,
|
||||
double dtSeconds) {
|
||||
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
|
||||
var discA = discABPair.getFirst();
|
||||
var discB = discABPair.getSecond();
|
||||
|
||||
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
|
||||
|
||||
var temp = discB.transpose().times(S).times(discB).plus(R);
|
||||
|
||||
m_K = temp.solve(discB.transpose().times(S).times(discA));
|
||||
|
||||
m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
|
||||
m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
|
||||
|
||||
reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param states The number of states.
|
||||
* @param inputs The number of inputs.
|
||||
* @param k The gain matrix.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public LinearQuadraticRegulator(
|
||||
Nat<States> states, Nat<Inputs> inputs, Matrix<Inputs, States> k) {
|
||||
m_K = k;
|
||||
|
||||
m_r = new Matrix<>(states, Nat.N1());
|
||||
m_u = new Matrix<>(inputs, Nat.N1());
|
||||
|
||||
reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the control input vector u.
|
||||
*
|
||||
* @return The control input.
|
||||
*/
|
||||
public Matrix<Inputs, N1> getU() {
|
||||
return m_u;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the control input vector u.
|
||||
*
|
||||
* @param row Row of u.
|
||||
* @return The row of the control input vector.
|
||||
*/
|
||||
public double getU(int row) {
|
||||
return m_u.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the reference vector r.
|
||||
*
|
||||
* @return The reference vector.
|
||||
*/
|
||||
public Matrix<States, N1> getR() {
|
||||
return m_r;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the reference vector r.
|
||||
*
|
||||
* @param row Row of r.
|
||||
* @return The row of the reference vector.
|
||||
*/
|
||||
public double getR(int row) {
|
||||
return m_r.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the controller matrix K.
|
||||
*
|
||||
* @return the controller matrix K.
|
||||
*/
|
||||
public Matrix<Inputs, States> getK() {
|
||||
return m_K;
|
||||
}
|
||||
|
||||
/** Resets the controller. */
|
||||
public void reset() {
|
||||
m_r.fill(0.0);
|
||||
m_u.fill(0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the controller.
|
||||
*
|
||||
* @param x The current state x.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public Matrix<Inputs, N1> calculate(Matrix<States, N1> x) {
|
||||
m_u = m_K.times(m_r.minus(x));
|
||||
return m_u;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the controller.
|
||||
*
|
||||
* @param x The current state x.
|
||||
* @param nextR the next reference vector r.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<States, N1> nextR) {
|
||||
m_r = nextR;
|
||||
return calculate(x);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adjusts LQR controller gain to compensate for a pure time delay in the input.
|
||||
*
|
||||
* <p>Linear-Quadratic regulator controller gains tend to be aggressive. If sensor measurements
|
||||
* are time-delayed too long, the LQR may be unstable. However, if we know the amount of delay, we
|
||||
* can compute the control based on where the system will be after the time delay.
|
||||
*
|
||||
* <p>See https://file.tavsys.net/control/controls-engineering-in-frc.pdf appendix C.4 for a
|
||||
* derivation.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param dtSeconds Discretization timestep in seconds.
|
||||
* @param inputDelaySeconds Input time delay in seconds.
|
||||
*/
|
||||
public void latencyCompensate(
|
||||
LinearSystem<States, Inputs, Outputs> plant, double dtSeconds, double inputDelaySeconds) {
|
||||
var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dtSeconds);
|
||||
var discA = discABPair.getFirst();
|
||||
var discB = discABPair.getSecond();
|
||||
|
||||
m_K = m_K.times((discA.minus(discB.times(m_K))).pow(inputDelaySeconds / dtSeconds));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,166 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.trajectory.Trajectory;
|
||||
|
||||
/**
|
||||
* Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model
|
||||
* to a desired pose along a two-dimensional trajectory. Why would we need a nonlinear control law
|
||||
* in addition to the linear ones we have used so far like PID? If we use the original approach with
|
||||
* PID controllers for left and right position and velocity states, the controllers only deal with
|
||||
* the local pose. If the robot deviates from the path, there is no way for the controllers to
|
||||
* correct and the robot may not reach the desired global pose. This is due to multiple endpoints
|
||||
* existing for the robot which have the same encoder path arc lengths.
|
||||
*
|
||||
* <p>Instead of using wheel path arc lengths (which are in the robot's local coordinate frame),
|
||||
* nonlinear controllers like pure pursuit and Ramsete use global pose. The controller uses this
|
||||
* extra information to guide a linear reference tracker like the PID controllers back in by
|
||||
* adjusting the references of the PID controllers.
|
||||
*
|
||||
* <p>The paper "Control of Wheeled Mobile Robots: An Experimental Overview" describes a nonlinear
|
||||
* controller for a wheeled vehicle with unicycle-like kinematics; a global pose consisting of x, y,
|
||||
* and theta; and a desired pose consisting of x_d, y_d, and theta_d. We call it Ramsete because
|
||||
* that's the acronym for the title of the book it came from in Italian ("Robotica Articolata e
|
||||
* Mobile per i SErvizi e le TEcnologie").
|
||||
*
|
||||
* <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
|
||||
* Engineering in the FIRST Robotics Competition</a> section on Ramsete unicycle controller for a
|
||||
* derivation and analysis.
|
||||
*/
|
||||
public class RamseteController {
|
||||
@SuppressWarnings("MemberName")
|
||||
private final double m_b;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
private final double m_zeta;
|
||||
|
||||
private Pose2d m_poseError = new Pose2d();
|
||||
private Pose2d m_poseTolerance = new Pose2d();
|
||||
private boolean m_enabled = true;
|
||||
|
||||
/**
|
||||
* Construct a Ramsete unicycle controller.
|
||||
*
|
||||
* @param b Tuning parameter (b > 0) for which larger values make convergence more aggressive
|
||||
* like a proportional term.
|
||||
* @param zeta Tuning parameter (0 < zeta < 1) for which larger values provide more damping
|
||||
* in response.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public RamseteController(double b, double zeta) {
|
||||
m_b = b;
|
||||
m_zeta = zeta;
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 and 0.7
|
||||
* have been well-tested to produce desirable results.
|
||||
*/
|
||||
public RamseteController() {
|
||||
this(2.0, 0.7);
|
||||
}
|
||||
|
||||
/** Returns true if the pose error is within tolerance of the reference. */
|
||||
public boolean atReference() {
|
||||
final var eTranslate = m_poseError.getTranslation();
|
||||
final var eRotate = m_poseError.getRotation();
|
||||
final var tolTranslate = m_poseTolerance.getTranslation();
|
||||
final var tolRotate = m_poseTolerance.getRotation();
|
||||
return Math.abs(eTranslate.getX()) < tolTranslate.getX()
|
||||
&& Math.abs(eTranslate.getY()) < tolTranslate.getY()
|
||||
&& Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the pose error which is considered tolerable for use with atReference().
|
||||
*
|
||||
* @param poseTolerance Pose error which is tolerable.
|
||||
*/
|
||||
public void setTolerance(Pose2d poseTolerance) {
|
||||
m_poseTolerance = poseTolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the Ramsete controller.
|
||||
*
|
||||
* <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
|
||||
* trajectory.
|
||||
*
|
||||
* @param currentPose The current pose.
|
||||
* @param poseRef The desired pose.
|
||||
* @param linearVelocityRefMeters The desired linear velocity in meters per second.
|
||||
* @param angularVelocityRefRadiansPerSecond The desired angular velocity in radians per second.
|
||||
*/
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public ChassisSpeeds calculate(
|
||||
Pose2d currentPose,
|
||||
Pose2d poseRef,
|
||||
double linearVelocityRefMeters,
|
||||
double angularVelocityRefRadiansPerSecond) {
|
||||
if (!m_enabled) {
|
||||
return new ChassisSpeeds(linearVelocityRefMeters, 0.0, angularVelocityRefRadiansPerSecond);
|
||||
}
|
||||
|
||||
m_poseError = poseRef.relativeTo(currentPose);
|
||||
|
||||
// Aliases for equation readability
|
||||
final double eX = m_poseError.getX();
|
||||
final double eY = m_poseError.getY();
|
||||
final double eTheta = m_poseError.getRotation().getRadians();
|
||||
final double vRef = linearVelocityRefMeters;
|
||||
final double omegaRef = angularVelocityRefRadiansPerSecond;
|
||||
|
||||
double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2));
|
||||
|
||||
return new ChassisSpeeds(
|
||||
vRef * m_poseError.getRotation().getCos() + k * eX,
|
||||
0.0,
|
||||
omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the Ramsete controller.
|
||||
*
|
||||
* <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
|
||||
* trajectory.
|
||||
*
|
||||
* @param currentPose The current pose.
|
||||
* @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
|
||||
*/
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
|
||||
return calculate(
|
||||
currentPose,
|
||||
desiredState.poseMeters,
|
||||
desiredState.velocityMetersPerSecond,
|
||||
desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
|
||||
}
|
||||
|
||||
/**
|
||||
* Enables and disables the controller for troubleshooting purposes.
|
||||
*
|
||||
* @param enabled If the controller is enabled or not.
|
||||
*/
|
||||
public void setEnabled(boolean enabled) {
|
||||
m_enabled = enabled;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns sin(x) / x.
|
||||
*
|
||||
* @param x Value of which to take sinc(x).
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
private static double sinc(double x) {
|
||||
if (Math.abs(x) < 1e-9) {
|
||||
return 1.0 - 1.0 / 6.0 * x * x;
|
||||
} else {
|
||||
return Math.sin(x) / x;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,121 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.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