Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Move math functionality into new wpimath library (#2629)

Browse Source 
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
2020-08-06 23:57:39 -07:00
committed by GitHub
parent ad817d4f23
commit 42993b15c6
463 changed files with 1006 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files

196
wpimath/src/main/native/include/frc/LinearFilter.h Normal file
View File

@@ -0,0 +1,196 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-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 <cassert>
#include <cmath>
#include <initializer_list>
#include <vector>
#include <wpi/ArrayRef.h>
#include <wpi/circular_buffer.h>
#include "math/MathShared.h"
#include "units/time.h"
namespace frc {
/**
* This class implements a linear, digital filter. All types of FIR and IIR
* filters are supported. Static factory methods are provided to create commonly
* used types of filters.
*
* Filters are of the form:<br>
* y[n] = (b0 * x[n] + b1 * x[n-1] + … + bP * x[n-P]) -
* (a0 * y[n-1] + a2 * y[n-2] + … + aQ * y[n-Q])
*
* Where:<br>
* y[n] is the output at time "n"<br>
* x[n] is the input at time "n"<br>
* y[n-1] is the output from the LAST time step ("n-1")<br>
* x[n-1] is the input from the LAST time step ("n-1")<br>
* b0 … bP are the "feedforward" (FIR) gains<br>
* a0 … aQ are the "feedback" (IIR) gains<br>
* IMPORTANT! Note the "-" sign in front of the feedback term! This is a common
* convention in signal processing.
*
* What can linear filters do? Basically, they can filter, or diminish, the
* effects of undesirable input frequencies. High frequencies, or rapid changes,
* can be indicative of sensor noise or be otherwise undesirable. A "low pass"
* filter smooths out the signal, reducing the impact of these high frequency
* components. Likewise, a "high pass" filter gets rid of slow-moving signal
* components, letting you detect large changes more easily.
*
* Example FRC applications of filters:
* - Getting rid of noise from an analog sensor input (note: the roboRIO's FPGA
* can do this faster in hardware)
* - Smoothing out joystick input to prevent the wheels from slipping or the
* robot from tipping
* - Smoothing motor commands so that unnecessary strain isn't put on
* electrical or mechanical components
* - If you use clever gains, you can make a PID controller out of this class!
*
* For more on filters, we highly recommend the following articles:<br>
* https://en.wikipedia.org/wiki/Linear_filter<br>
* https://en.wikipedia.org/wiki/Iir_filter<br>
* https://en.wikipedia.org/wiki/Fir_filter<br>
*
* Note 1: Calculate() should be called by the user on a known, regular period.
* You can use a Notifier for this or do it "inline" with code in a
* periodic function.
*
* Note 2: For ALL filters, gains are necessarily a function of frequency. If
* you make a filter that works well for you at, say, 100Hz, you will most
* definitely need to adjust the gains if you then want to run it at 200Hz!
* Combining this with Note 1 - the impetus is on YOU as a developer to make
* sure Calculate() gets called at the desired, constant frequency!
*/
template <class T>
class LinearFilter {
public:
/**
* Create a linear FIR or IIR filter.
*
* @param ffGains The "feed forward" or FIR gains.
* @param fbGains The "feed back" or IIR gains.
*/
LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> fbGains)
: m_inputs(ffGains.size()),
m_outputs(fbGains.size()),
m_inputGains(ffGains),
m_outputGains(fbGains) {
static int instances = 0;
instances++;
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kFilter_Linear, 1);
}
/**
* Create a linear FIR or IIR filter.
*
* @param ffGains The "feed forward" or FIR gains.
* @param fbGains The "feed back" or IIR gains.
*/
LinearFilter(std::initializer_list<double> ffGains,
std::initializer_list<double> fbGains)
: LinearFilter(wpi::makeArrayRef(ffGains.begin(), ffGains.end()),
wpi::makeArrayRef(fbGains.begin(), fbGains.end())) {}
// Static methods to create commonly used filters
/**
* Creates a one-pole IIR low-pass filter of the form:<br>
* y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>
* where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
*
* This filter is stable for time constants greater than zero.
*
* @param timeConstant The discrete-time time constant in seconds.
* @param period The period in seconds between samples taken by the
* user.
*/
static LinearFilter<T> SinglePoleIIR(double timeConstant,
units::second_t period) {
double gain = std::exp(-period.to<double>() / timeConstant);
return LinearFilter(1.0 - gain, -gain);
}
/**
* Creates a first-order high-pass filter of the form:<br>
* y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]<br>
* where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
*
* This filter is stable for time constants greater than zero.
*
* @param timeConstant The discrete-time time constant in seconds.
* @param period The period in seconds between samples taken by the
* user.
*/
static LinearFilter<T> HighPass(double timeConstant, units::second_t period) {
double gain = std::exp(-period.to<double>() / timeConstant);
return LinearFilter({gain, -gain}, {-gain});
}
/**
* Creates a K-tap FIR moving average filter of the form:<br>
* y[n] = 1/k * (x[k] + x[k-1] + … + x[0])
*
* This filter is always stable.
*
* @param taps The number of samples to average over. Higher = smoother but
* slower
*/
static LinearFilter<T> MovingAverage(int taps) {
assert(taps > 0);
std::vector<double> gains(taps, 1.0 / taps);
return LinearFilter(gains, {});
}
/**
* Reset the filter state.
*/
void Reset() {
m_inputs.reset();
m_outputs.reset();
}
/**
* Calculates the next value of the filter.
*
* @param input Current input value.
*
* @return The filtered value at this step
*/
T Calculate(T input) {
T retVal = T(0.0);
// Rotate the inputs
m_inputs.push_front(input);
// Calculate the new value
for (size_t i = 0; i < m_inputGains.size(); i++) {
retVal += m_inputs[i] * m_inputGains[i];
}
for (size_t i = 0; i < m_outputGains.size(); i++) {
retVal -= m_outputs[i] * m_outputGains[i];
}
// Rotate the outputs
m_outputs.push_front(retVal);
return retVal;
}
private:
wpi::circular_buffer<T> m_inputs;
wpi::circular_buffer<T> m_outputs;
std::vector<double> m_inputGains;
std::vector<double> m_outputGains;
};
} // namespace frc

80
wpimath/src/main/native/include/frc/MedianFilter.h Normal file
View File

@@ -0,0 +1,80 @@
/*----------------------------------------------------------------------------*/
/* 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 <algorithm>
#include <vector>
#include <wpi/Algorithm.h>
#include <wpi/circular_buffer.h>
namespace frc {
/**
* A class that implements a moving-window median filter. Useful for reducing
* measurement noise, especially with processes that generate occasional,
* extreme outliers (such as values from vision processing, LIDAR, or ultrasonic
* sensors).
*/
template <class T>
class MedianFilter {
public:
/**
* Creates a new MedianFilter.
*
* @param size The number of samples in the moving window.
*/
explicit MedianFilter(size_t size) : m_valueBuffer(size), m_size{size} {}
/**
* Calculates the moving-window median for the next value of the input stream.
*
* @param next The next input value.
* @return The median of the moving window, updated to include the next value.
*/
T Calculate(T next) {
// Insert next value at proper point in sorted array
wpi::insert_sorted(m_orderedValues, next);
size_t curSize = m_orderedValues.size();
// If buffer is at max size, pop element off of end of circular buffer
// and remove from ordered list
if (curSize > m_size) {
m_orderedValues.erase(std::find(m_orderedValues.begin(),
m_orderedValues.end(),
m_valueBuffer.pop_back()));
curSize = curSize - 1;
}
// Add next value to circular buffer
m_valueBuffer.push_front(next);
if (curSize % 2 == 1) {
// If size is odd, return middle element of sorted list
return m_orderedValues[curSize / 2];
} else {
// If size is even, return average of middle elements
return (m_orderedValues[curSize / 2 - 1] + m_orderedValues[curSize / 2]) /
2.0;
}
}
/**
* Resets the filter, clearing the window of all elements.
*/
void Reset() {
m_orderedValues.clear();
m_valueBuffer.reset();
}
private:
wpi::circular_buffer<T> m_valueBuffer;
std::vector<T> m_orderedValues;
size_t m_size;
};
} // namespace frc

153
wpimath/src/main/native/include/frc/controller/ArmFeedforward.h Normal file
View File

@@ -0,0 +1,153 @@
/*----------------------------------------------------------------------------*/
/* 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 <wpi/MathExtras.h>
#include "units/angle.h"
#include "units/angular_velocity.h"
#include "units/math.h"
#include "units/voltage.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

132
wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h Normal file
View File

@@ -0,0 +1,132 @@
/*----------------------------------------------------------------------------*/
/* 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 <wpi/MathExtras.h>
#include "units/voltage.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

133
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h Normal file
View File

@@ -0,0 +1,133 @@
/*----------------------------------------------------------------------------*/
/* 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 <wpi/MathExtras.h>
#include "units/time.h"
#include "units/voltage.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

193
wpimath/src/main/native/include/frc/geometry/Pose2d.h Normal file
View File

@@ -0,0 +1,193 @@
/*----------------------------------------------------------------------------*/
/* 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 "Transform2d.h"
#include "Translation2d.h"
#include "Twist2d.h"
namespace wpi {
class json;
} // namespace wpi
namespace frc {
/**
* Represents a 2d pose containing translational and rotational elements.
*/
class Pose2d {
public:
/**
* Constructs a pose at the origin facing toward the positive X axis.
* (Translation2d{0, 0} and Rotation{0})
*/
constexpr Pose2d() = default;
/**
* Constructs a pose with the specified translation and rotation.
*
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose2d(Translation2d translation, Rotation2d rotation);
/**
* Convenience constructors that takes in x and y values directly instead of
* having to construct a Translation2d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
/**
* Transforms the pose by the given transformation and returns the new
* transformed pose.
*
* [x_new] [cos, -sin, 0][transform.x]
* [y_new] += [sin, cos, 0][transform.y]
* [t_new] [0, 0, 1][transform.t]
*
* @param other The transform to transform the pose by.
*
* @return The transformed pose.
*/
Pose2d operator+(const Transform2d& other) const;
/**
* Transforms the current pose by the transformation.
*
* This is similar to the + operator, except that it mutates the current
* object.
*
* @param other The transform to transform the pose by.
*
* @return Reference to the new mutated object.
*/
Pose2d& operator+=(const Transform2d& other);
/**
* Returns the Transform2d that maps the one pose to another.
*
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
Transform2d operator-(const Pose2d& other) const;
/**
* Checks equality between this Pose2d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Pose2d& other) const;
/**
* Checks inequality between this Pose2d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Pose2d& other) const;
/**
* Returns the underlying translation.
*
* @return Reference to the translational component of the pose.
*/
const Translation2d& Translation() const { return m_translation; }
/**
* Returns the X component of the pose's translation.
*
* @return The x component of the pose's translation.
*/
units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the underlying rotation.
*
* @return Reference to the rotational component of the pose.
*/
const Rotation2d& Rotation() const { return m_rotation; }
/**
* Transforms the pose by the given transformation and returns the new pose.
* See + operator for the matrix multiplication performed.
*
* @param other The transform to transform the pose by.
*
* @return The transformed pose.
*/
Pose2d TransformBy(const Transform2d& other) const;
/**
* Returns the other pose relative to the current pose.
*
* This function can often be used for trajectory tracking or pose
* stabilization algorithms to get the error between the reference and the
* current pose.
*
* @param other The pose that is the origin of the new coordinate frame that
* the current pose will be converted into.
*
* @return The current pose relative to the new origin pose.
*/
Pose2d RelativeTo(const Pose2d& other) const;
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
*
* See <https://file.tavsys.net/control/state-space-guide.pdf> section on
* nonlinear pose estimation for derivation.
*
* The twist is a change in pose in the robot's coordinate frame since the
* previous pose update. When the user runs exp() on the previous known
* field-relative pose with the argument being the twist, the user will
* receive the new field-relative pose.
*
* "Exp" represents the pose exponential, which is solving a differential
* equation moving the pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the
* previous pose update. For example, if a non-holonomic robot moves forward
* 0.01 meters and changes angle by 0.5 degrees since the previous pose
* update, the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
*
* @return The new pose of the robot.
*/
Pose2d Exp(const Twist2d& twist) const;
/**
* Returns a Twist2d that maps this pose to the end pose. If c is the output
* of a.Log(b), then a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
*
* @return The twist that maps this to end.
*/
Twist2d Log(const Pose2d& end) const;
private:
Translation2d m_translation;
Rotation2d m_rotation;
};
void to_json(wpi::json& json, const Pose2d& pose);
void from_json(const wpi::json& json, Pose2d& pose);
} // namespace frc

193
wpimath/src/main/native/include/frc/geometry/Rotation2d.h Normal file
View File

@@ -0,0 +1,193 @@
/*----------------------------------------------------------------------------*/
/* 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>
namespace wpi {
class json;
} // namespace wpi
namespace frc {
/**
* A rotation in a 2d coordinate frame represented a point on the unit circle
* (cosine and sine).
*/
class Rotation2d {
public:
/**
* Constructs a Rotation2d with a default angle of 0 degrees.
*/
constexpr Rotation2d() = default;
/**
* Constructs a Rotation2d with the given radian value.
*
* @param value The value of the angle in radians.
*/
Rotation2d(units::radian_t value); // NOLINT(runtime/explicit)
/**
* Constructs a Rotation2d with the given degree value.
*
* @param value The value of the angle in degrees.
*/
Rotation2d(units::degree_t value); // NOLINT(runtime/explicit)
/**
* Constructs a Rotation2d with the given x and y (cosine and sine)
* components. The x and y don't have to be normalized.
*
* @param x The x component or cosine of the rotation.
* @param y The y component or sine of the rotation.
*/
Rotation2d(double x, double y);
/**
* Adds two rotations together, with the result being bounded between -pi and
* pi.
*
* For example, Rotation2d.FromDegrees(30) + Rotation2d.FromDegrees(60) =
* Rotation2d{-pi/2}
*
* @param other The rotation to add.
*
* @return The sum of the two rotations.
*/
Rotation2d operator+(const Rotation2d& other) const;
/**
* Adds a rotation to the current rotation.
*
* This is similar to the + operator except that it mutates the current
* object.
*
* @param other The rotation to add.
*
* @return The reference to the new mutated object.
*/
Rotation2d& operator+=(const Rotation2d& other);
/**
* Subtracts the new rotation from the current rotation and returns the new
* rotation.
*
* For example, Rotation2d.FromDegrees(10) - Rotation2d.FromDegrees(100) =
* Rotation2d{-pi/2}
*
* @param other The rotation to subtract.
*
* @return The difference between the two rotations.
*/
Rotation2d operator-(const Rotation2d& other) const;
/**
* Subtracts the new rotation from the current rotation.
*
* This is similar to the - operator except that it mutates the current
* object.
*
* @param other The rotation to subtract.
*
* @return The reference to the new mutated object.
*/
Rotation2d& operator-=(const Rotation2d& other);
/**
* Takes the inverse of the current rotation. This is simply the negative of
* the current angular value.
*
* @return The inverse of the current rotation.
*/
Rotation2d operator-() const;
/**
* Multiplies the current rotation by a scalar.
* @param scalar The scalar.
*
* @return The new scaled Rotation2d.
*/
Rotation2d operator*(double scalar) const;
/**
* Checks equality between this Rotation2d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Rotation2d& other) const;
/**
* Checks inequality between this Rotation2d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Rotation2d& other) const;
/**
* Adds the new rotation to the current rotation using a rotation matrix.
*
* [cos_new] [other.cos, -other.sin][cos]
* [sin_new] = [other.sin, other.cos][sin]
*
* value_new = std::atan2(cos_new, sin_new)
*
* @param other The rotation to rotate by.
*
* @return The new rotated Rotation2d.
*/
Rotation2d RotateBy(const Rotation2d& other) const;
/**
* Returns the radian value of the rotation.
*
* @return The radian value of the rotation.
*/
units::radian_t Radians() const { return m_value; }
/**
* Returns the degree value of the rotation.
*
* @return The degree value of the rotation.
*/
units::degree_t Degrees() const { return m_value; }
/**
* Returns the cosine of the rotation.
*
* @return The cosine of the rotation.
*/
double Cos() const { return m_cos; }
/**
* Returns the sine of the rotation.
*
* @return The sine of the rotation.
*/
double Sin() const { return m_sin; }
/**
* Returns the tangent of the rotation.
*
* @return The tangent of the rotation.
*/
double Tan() const { return m_sin / m_cos; }
private:
units::radian_t m_value = 0_rad;
double m_cos = 1;
double m_sin = 0;
};
void to_json(wpi::json& json, const Rotation2d& rotation);
void from_json(const wpi::json& json, Rotation2d& rotation);
} // namespace frc

107
wpimath/src/main/native/include/frc/geometry/Transform2d.h Normal file
View File

@@ -0,0 +1,107 @@
/*----------------------------------------------------------------------------*/
/* 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 "Translation2d.h"
namespace frc {
class Pose2d;
/**
* Represents a transformation for a Pose2d.
*/
class Transform2d {
public:
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param final The final pose for the transformation.
*/
Transform2d(Pose2d initial, Pose2d final);
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
Transform2d(Translation2d translation, Rotation2d rotation);
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
constexpr Transform2d() = default;
/**
* Returns the translation component of the transformation.
*
* @return Reference to the translational component of the transform.
*/
const Translation2d& Translation() const { return m_translation; }
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
const Rotation2d& Rotation() const { return m_rotation; }
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
Transform2d Inverse() const;
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform2d.
*/
Transform2d operator*(double scalar) const {
return Transform2d(m_translation * scalar, m_rotation * scalar);
}
/**
* Checks equality between this Transform2d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Transform2d& other) const;
/**
* Checks inequality between this Transform2d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Transform2d& other) const;
private:
Translation2d m_translation;
Rotation2d m_rotation;
};
} // namespace frc

223
wpimath/src/main/native/include/frc/geometry/Translation2d.h Normal file
View File

@@ -0,0 +1,223 @@
/*----------------------------------------------------------------------------*/
/* 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/length.h>
#include "Rotation2d.h"
namespace wpi {
class json;
} // namespace wpi
namespace frc {
/**
* Represents a translation in 2d space.
* This object can be used to represent a point or a vector.
*
* This assumes that you are using conventional mathematical axes.
* When the robot is placed on the origin, facing toward the X direction,
* moving forward increases the X, whereas moving to the left increases the Y.
*/
class Translation2d {
public:
/**
* Constructs a Translation2d with X and Y components equal to zero.
*/
constexpr Translation2d() = default;
/**
* Constructs a Translation2d with the X and Y components equal to the
* provided values.
*
* @param x The x component of the translation.
* @param y The y component of the translation.
*/
Translation2d(units::meter_t x, units::meter_t y);
/**
* Calculates the distance between two translations in 2d space.
*
* This function uses the pythagorean theorem to calculate the distance.
* distance = std::sqrt((x2 - x1)^2 + (y2 - y1)^2)
*
* @param other The translation to compute the distance to.
*
* @return The distance between the two translations.
*/
units::meter_t Distance(const Translation2d& other) const;
/**
* Returns the X component of the translation.
*
* @return The x component of the translation.
*/
units::meter_t X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
* @return The y component of the translation.
*/
units::meter_t Y() const { return m_y; }
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
units::meter_t Norm() const;
/**
* Applies a rotation to the translation in 2d space.
*
* This multiplies the translation vector by a counterclockwise rotation
* matrix of the given angle.
*
* [x_new] [other.cos, -other.sin][x]
* [y_new] = [other.sin, other.cos][y]
*
* For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
* Translation2d of {0, 2}.
*
* @param other The rotation to rotate the translation by.
*
* @return The new rotated translation.
*/
Translation2d RotateBy(const Rotation2d& other) const;
/**
* Adds two translations in 2d space and returns the sum. This is similar to
* vector addition.
*
* For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
* Translation2d{3.0, 8.0}
*
* @param other The translation to add.
*
* @return The sum of the translations.
*/
Translation2d operator+(const Translation2d& other) const;
/**
* Adds the new translation to the current translation.
*
* This is similar to the + operator, except that the current object is
* mutated.
*
* @param other The translation to add.
*
* @return The reference to the new mutated object.
*/
Translation2d& operator+=(const Translation2d& other);
/**
* Subtracts the other translation from the other translation and returns the
* difference.
*
* For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
* Translation2d{4.0, 2.0}
*
* @param other The translation to subtract.
*
* @return The difference between the two translations.
*/
Translation2d operator-(const Translation2d& other) const;
/**
* Subtracts the new translation from the current translation.
*
* This is similar to the - operator, except that the current object is
* mutated.
*
* @param other The translation to subtract.
*
* @return The reference to the new mutated object.
*/
Translation2d& operator-=(const Translation2d& other);
/**
* Returns the inverse of the current translation. This is equivalent to
* rotating by 180 degrees, flipping the point over both axes, or simply
* negating both components of the translation.
*
* @return The inverse of the current translation.
*/
Translation2d operator-() const;
/**
* Multiplies the translation by a scalar and returns the new translation.
*
* For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
*
* @param scalar The scalar to multiply by.
*
* @return The scaled translation.
*/
Translation2d operator*(double scalar) const;
/**
* Multiplies the current translation by a scalar.
*
* This is similar to the * operator, except that current object is mutated.
*
* @param scalar The scalar to multiply by.
*
* @return The reference to the new mutated object.
*/
Translation2d& operator*=(double scalar);
/**
* Divides the translation by a scalar and returns the new translation.
*
* For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
*
* @param scalar The scalar to divide by.
*
* @return The scaled translation.
*/
Translation2d operator/(double scalar) const;
/**
* Checks equality between this Translation2d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Translation2d& other) const;
/**
* Checks inequality between this Translation2d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Translation2d& other) const;
/*
* Divides the current translation by a scalar.
*
* This is similar to the / operator, except that current object is mutated.
*
* @param scalar The scalar to divide by.
*
* @return The reference to the new mutated object.
*/
Translation2d& operator/=(double scalar);
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;
};
void to_json(wpi::json& json, const Translation2d& state);
void from_json(const wpi::json& json, Translation2d& state);
} // namespace frc

58
wpimath/src/main/native/include/frc/geometry/Twist2d.h Normal file
View File

@@ -0,0 +1,58 @@
/*----------------------------------------------------------------------------*/
/* 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/length.h>
#include <units/math.h>
namespace frc {
/**
* A change in distance along arc since the last pose update. We can use ideas
* from differential calculus to create new Pose2ds from a Twist2d and vise
* versa.
*
* A Twist can be used to represent a difference between two poses.
*/
struct Twist2d {
/**
* Linear "dx" component
*/
units::meter_t dx = 0_m;
/**
* Linear "dy" component
*/
units::meter_t dy = 0_m;
/**
* Angular "dtheta" component (radians)
*/
units::radian_t dtheta = 0_rad;
/**
* Checks equality between this Twist2d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Twist2d& other) const {
return units::math::abs(dx - other.dx) < 1E-9_m &&
units::math::abs(dy - other.dy) < 1E-9_m &&
units::math::abs(dtheta - other.dtheta) < 1E-9_rad;
}
/**
* Checks inequality between this Twist2d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Twist2d& other) const { return !operator==(other); }
};
} // namespace frc

65
wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h Normal file
View File

@@ -0,0 +1,65 @@
/*----------------------------------------------------------------------------*/
/* 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 "frc/geometry/Rotation2d.h"
#include "units/angular_velocity.h"
#include "units/velocity.h"
namespace frc {
/**
* Represents the speed of a robot chassis. Although this struct contains
* similar members compared to a Twist2d, they do NOT represent the same thing.
* Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
* reference, this ChassisSpeeds struct represents a velocity w.r.t to the robot
* frame of reference.
*
* A strictly non-holonomic drivetrain, such as a differential drive, should
* never have a dy component because it can never move sideways. Holonomic
* drivetrains such as swerve and mecanum will often have all three components.
*/
struct ChassisSpeeds {
/**
* Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
*/
units::meters_per_second_t vx = 0_mps;
/**
* Represents strafe velocity w.r.t the robot frame of reference. (Left is +)
*/
units::meters_per_second_t vy = 0_mps;
/**
* Represents the angular velocity of the robot frame. (CCW is +)
*/
units::radians_per_second_t omega = 0_rad_per_s;
/**
* Converts a user provided field-relative set of speeds into a robot-relative
* ChassisSpeeds object.
*
* @param vx The component of speed in the x direction relative to the field.
* Positive x is away from your alliance wall.
* @param vy The component of speed in the y direction relative to the field.
* Positive y is to your left when standing behind the alliance wall.
* @param omega The angular rate of the robot.
* @param robotAngle The angle of the robot as measured by a gyroscope. The
* robot's angle is considered to be zero when it is facing directly away from
* your alliance station wall. Remember that this should be CCW positive.
*
* @return ChassisSpeeds object representing the speeds in the robot's frame
* of reference.
*/
static ChassisSpeeds FromFieldRelativeSpeeds(
units::meters_per_second_t vx, units::meters_per_second_t vy,
units::radians_per_second_t omega, const Rotation2d& robotAngle) {
return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
-vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
}
};
} // namespace frc

70
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h Normal file
View File

@@ -0,0 +1,70 @@
/*----------------------------------------------------------------------------*/
/* 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 "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "math/MathShared.h"
#include "units/angle.h"
#include "units/length.h"
namespace frc {
/**
* Helper class that converts a chassis velocity (dx and dtheta components) to
* left and right wheel velocities for a differential drive.
*
* Inverse kinematics converts a desired chassis speed into left and right
* velocity components whereas forward kinematics converts left and right
* component velocities into a linear and angular chassis speed.
*/
class DifferentialDriveKinematics {
public:
/**
* Constructs a differential drive kinematics object.
*
* @param trackWidth The track width of the drivetrain. Theoretically, this is
* the distance between the left wheels and right wheels. However, the
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
*/
explicit DifferentialDriveKinematics(units::meter_t trackWidth)
: trackWidth(trackWidth) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kKinematics_DifferentialDrive, 1);
}
/**
* Returns a chassis speed from left and right component velocities using
* forward kinematics.
*
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
*/
constexpr ChassisSpeeds ToChassisSpeeds(
const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
(wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
}
/**
* Returns left and right component velocities from a chassis speed using
* inverse kinematics.
*
* @param chassisSpeeds The linear and angular (dx and dtheta) components that
* represent the chassis' speed.
* @return The left and right velocities.
*/
constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const {
return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
}
units::meter_t trackWidth;
};
} // namespace frc

87
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h Normal file
View File

@@ -0,0 +1,87 @@
/*----------------------------------------------------------------------------*/
/* 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 "DifferentialDriveKinematics.h"
#include "frc/geometry/Pose2d.h"
#include "units/length.h"
namespace frc {
/**
* Class for differential drive odometry. Odometry allows you to track the
* robot's position on the field over the course of a match using readings from
* 2 encoders and a gyroscope.
*
* Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*
* It is important that you reset your encoders to zero before using this class.
* Any subsequent pose resets also require the encoders to be reset to zero.
*/
class DifferentialDriveOdometry {
public:
/**
* Constructs a DifferentialDriveOdometry object.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
* You NEED to reset your encoders (to zero) when calling this method.
*
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_prevLeftDistance = 0_m;
m_prevRightDistance = 0_m;
}
/**
* Returns the position of the robot on the field.
* @return The pose of the robot.
*/
const Pose2d& GetPose() const { return m_pose; }
/**
* Updates the robot position on the field using distance measurements from
* encoders. This method is more numerically accurate than using velocities to
* integrate the pose and is also advantageous for teams that are using lower
* CPR encoders.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance);
private:
Pose2d m_pose;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
units::meter_t m_prevLeftDistance = 0_m;
units::meter_t m_prevRightDistance = 0_m;
};
} // namespace frc

39
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h Normal file
View File

@@ -0,0 +1,39 @@
/*----------------------------------------------------------------------------*/
/* 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/velocity.h"
namespace frc {
/**
* Represents the wheel speeds for a differential drive drivetrain.
*/
struct DifferentialDriveWheelSpeeds {
/**
* Speed of the left side of the robot.
*/
units::meters_per_second_t left = 0_mps;
/**
* Speed of the right side of the robot.
*/
units::meters_per_second_t right = 0_mps;
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
* this issue, one can "normalize" all the wheel speeds to make sure that all
* requested module speeds are below the absolute threshold, while maintaining
* the ratio of speeds between modules.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
void Normalize(units::meters_per_second_t attainableMaxSpeed);
};
} // namespace frc

145
wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h Normal file
View File

@@ -0,0 +1,145 @@
/*----------------------------------------------------------------------------*/
/* 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 <Eigen/Core>
#include <Eigen/QR>
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include "math/MathShared.h"
namespace frc {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual wheel speeds.
*
* The inverse kinematics (converting from a desired chassis velocity to
* individual wheel speeds) uses the relative locations of the wheels with
* respect to the center of rotation. The center of rotation for inverse
* kinematics is also variable. This means that you can set your set your center
* of rotation in a corner of the robot to perform special evasion manuevers.
*
* Forward kinematics (converting an array of wheel speeds into the overall
* chassis motion) is performs the exact opposite of what inverse kinematics
* does. Since this is an overdetermined system (more equations than variables),
* we use a least-squares approximation.
*
* The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
* We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
* multiply by [wheelSpeeds] to get our chassis speeds.
*
* Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
*/
class MecanumDriveKinematics {
public:
/**
* Constructs a mecanum drive kinematics object.
*
* @param frontLeftWheel The location of the front-left wheel relative to the
* physical center of the robot.
* @param frontRightWheel The location of the front-right wheel relative to
* the physical center of the robot.
* @param rearLeftWheel The location of the rear-left wheel relative to the
* physical center of the robot.
* @param rearRightWheel The location of the rear-right wheel relative to the
* physical center of the robot.
*/
explicit MecanumDriveKinematics(Translation2d frontLeftWheel,
Translation2d frontRightWheel,
Translation2d rearLeftWheel,
Translation2d rearRightWheel)
: m_frontLeftWheel{frontLeftWheel},
m_frontRightWheel{frontRightWheel},
m_rearLeftWheel{rearLeftWheel},
m_rearRightWheel{rearRightWheel} {
SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel,
rearRightWheel);
m_forwardKinematics = m_inverseKinematics.householderQr();
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kKinematics_MecanumDrive, 1);
}
MecanumDriveKinematics(const MecanumDriveKinematics&) = default;
/**
* Performs inverse kinematics to return the wheel speeds from a desired
* chassis velocity. This method is often used to convert joystick values into
* wheel speeds.
*
* This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
* center of the robot; therefore, the argument is defaulted to that use case.
* However, if you wish to change the center of rotation for evasive
* manuevers, vision alignment, or for any other use case, you can do so.
*
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotation The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and
* provide a chassis speed that only has a dtheta
* component, the robot will rotate around that
* corner.
*
* @return The wheel speeds. Use caution because they are not normalized.
* Sometimes, a user input may cause one of the wheel speeds to go
* above the attainable max velocity. Use the
* MecanumDriveWheelSpeeds::Normalize() function to rectify this
* issue. In addition, you can leverage the power of C++17 to directly
* assign the wheel speeds to variables:
*
* @code{.cpp}
* auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(chassisSpeeds);
* @endcode
*/
MecanumDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation = Translation2d()) const;
/**
* Performs forward kinematics to return the resulting chassis state from the
* given wheel speeds. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed of
* each wheel on the robot.
*
* @param wheelSpeeds The current mecanum drive wheel speeds.
*
* @return The resulting chassis speed.
*/
ChassisSpeeds ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const;
private:
mutable Eigen::Matrix<double, 4, 3> m_inverseKinematics;
Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
Translation2d m_frontLeftWheel;
Translation2d m_frontRightWheel;
Translation2d m_rearLeftWheel;
Translation2d m_rearRightWheel;
mutable Translation2d m_previousCoR;
/**
* Construct inverse kinematics matrix from wheel locations.
*
* @param fl The location of the front-left wheel relative to the physical
* center of the robot.
* @param fr The location of the front-right wheel relative to the physical
* center of the robot.
* @param rl The location of the rear-left wheel relative to the physical
* center of the robot.
* @param rr The location of the rear-right wheel relative to the physical
* center of the robot.
*/
void SetInverseKinematics(Translation2d fl, Translation2d fr,
Translation2d rl, Translation2d rr) const;
};
} // namespace frc

107
wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h Normal file
View File

@@ -0,0 +1,107 @@
/*----------------------------------------------------------------------------*/
/* 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 <wpi/timestamp.h>
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include "units/time.h"
namespace frc {
/**
* Class for mecanum drive odometry. Odometry allows you to track the robot's
* position on the field over a course of a match using readings from your
* mecanum wheel encoders.
*
* Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*/
class MecanumDriveOdometry {
public:
/**
* Constructs a MecanumDriveOdometry object.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
/**
* Returns the position of the robot on the field.
* @return The pose of the robot.
*/
const Pose2d& GetPose() const { return m_pose; }
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as
* a parameter to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds);
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates
* the current time to calculate period (difference between two timestamps).
* The period is used to calculate the change in distance from a velocity.
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, wheelSpeeds);
}
private:
MecanumDriveKinematics m_kinematics;
Pose2d m_pose;
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
};
} // namespace frc

49
wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h Normal file
View File

@@ -0,0 +1,49 @@
/*----------------------------------------------------------------------------*/
/* 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/velocity.h"
namespace frc {
/**
* Represents the wheel speeds for a mecanum drive drivetrain.
*/
struct MecanumDriveWheelSpeeds {
/**
* Speed of the front-left wheel.
*/
units::meters_per_second_t frontLeft = 0_mps;
/**
* Speed of the front-right wheel.
*/
units::meters_per_second_t frontRight = 0_mps;
/**
* Speed of the rear-left wheel.
*/
units::meters_per_second_t rearLeft = 0_mps;
/**
* Speed of the rear-right wheel.
*/
units::meters_per_second_t rearRight = 0_mps;
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
* this issue, one can "normalize" all the wheel speeds to make sure that all
* requested module speeds are below the absolute threshold, while maintaining
* the ratio of speeds between modules.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
void Normalize(units::meters_per_second_t attainableMaxSpeed);
};
} // namespace frc

170
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h Normal file
View File

@@ -0,0 +1,170 @@
/*----------------------------------------------------------------------------*/
/* 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 <array>
#include <cstddef>
#include <Eigen/Core>
#include <Eigen/QR>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/SwerveModuleState.h"
#include "math/MathShared.h"
#include "units/velocity.h"
namespace frc {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual module states (speed and angle).
*
* The inverse kinematics (converting from a desired chassis velocity to
* individual module states) uses the relative locations of the modules with
* respect to the center of rotation. The center of rotation for inverse
* kinematics is also variable. This means that you can set your set your center
* of rotation in a corner of the robot to perform special evasion manuevers.
*
* Forward kinematics (converting an array of module states into the overall
* chassis motion) is performs the exact opposite of what inverse kinematics
* does. Since this is an overdetermined system (more equations than variables),
* we use a least-squares approximation.
*
* The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
* We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
* multiply by [moduleStates] to get our chassis speeds.
*
* Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
*/
template <size_t NumModules>
class SwerveDriveKinematics {
public:
/**
* Constructs a swerve drive kinematics object. This takes in a variable
* number of wheel locations as Translation2ds. The order in which you pass in
* the wheel locations is the same order that you will recieve the module
* states when performing inverse kinematics. It is also expected that you
* pass in the module states in the same order when calling the forward
* kinematics methods.
*
* @param wheels The locations of the wheels relative to the physical center
* of the robot.
*/
template <typename... Wheels>
explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
: m_modules{wheel, wheels...} {
static_assert(sizeof...(wheels) >= 1,
"A swerve drive requires at least two modules");
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
1, 0, (-m_modules[i].Y()).template to<double>(),
0, 1, (+m_modules[i].X()).template to<double>();
// clang-format on
}
m_forwardKinematics = m_inverseKinematics.householderQr();
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kKinematics_SwerveDrive, 1);
}
SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
/**
* Performs inverse kinematics to return the module states from a desired
* chassis velocity. This method is often used to convert joystick values into
* module speeds and angles.
*
* This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
* center of the robot; therefore, the argument is defaulted to that use case.
* However, if you wish to change the center of rotation for evasive
* manuevers, vision alignment, or for any other use case, you can do so.
*
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotation The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and provide a chassis speed
* that only has a dtheta component, the robot will rotate around that corner.
*
* @return An array containing the module states. Use caution because these
* module states are not normalized. Sometimes, a user input may cause one of
* the module speeds to go above the attainable max velocity. Use the
* <NormalizeWheelSpeeds> function to rectify this issue. In addition, you can
* leverage the power of C++17 to directly assign the module states to
* variables:
*
* @code{.cpp}
* auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds);
* @endcode
*/
std::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation = Translation2d()) const;
/**
* Performs forward kinematics to return the resulting chassis state from the
* given module states. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed and
* angle of each module on the robot.
*
* @param wheelStates The state of the modules (as a SwerveModuleState type)
* as measured from respective encoders and gyros. The order of the swerve
* module states should be same as passed into the constructor of this class.
*
* @return The resulting chassis speed.
*/
template <typename... ModuleStates>
ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates) const;
/**
* Performs forward kinematics to return the resulting chassis state from the
* given module states. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed and
* angle of each module on the robot.
*
* @param moduleStates The state of the modules as an std::array of type
* SwerveModuleState, NumModules long as measured from respective encoders
* and gyros. The order of the swerve module states should be same as passed
* into the constructor of this class.
*
* @return The resulting chassis speed.
*/
ChassisSpeeds ToChassisSpeeds(
std::array<SwerveModuleState, NumModules> moduleStates) const;
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
* this issue, one can "normalize" all the wheel speeds to make sure that all
* requested module speeds are below the absolute threshold, while maintaining
* the ratio of speeds between modules.
*
* @param moduleStates Reference to array of module states. The array will be
* mutated with the normalized speeds!
* @param attainableMaxSpeed The absolute max speed that a module can reach.
*/
static void NormalizeWheelSpeeds(
std::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed);
private:
mutable Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
m_forwardKinematics;
std::array<Translation2d, NumModules> m_modules;
mutable Translation2d m_previousCoR;
};
} // namespace frc
#include "SwerveDriveKinematics.inc"

113
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc Normal file
View File

@@ -0,0 +1,113 @@
/*----------------------------------------------------------------------------*/
/* 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 <algorithm>
#include "units/math.h"
namespace frc {
template <class... Wheels>
SwerveDriveKinematics(Translation2d, Wheels...)
-> SwerveDriveKinematics<1 + sizeof...(Wheels)>;
template <size_t NumModules>
std::array<SwerveModuleState, NumModules>
SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation) const {
// We have a new center of rotation. We need to compute the matrix again.
if (centerOfRotation != m_previousCoR) {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).template to<double>(),
0, 1, (+m_modules[i].X() - centerOfRotation.X()).template to<double>();
// clang-format on
}
m_previousCoR = centerOfRotation;
}
Eigen::Vector3d chassisSpeedsVector;
chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
m_inverseKinematics * chassisSpeedsVector;
std::array<SwerveModuleState, NumModules> moduleStates;
for (size_t i = 0; i < NumModules; i++) {
units::meters_per_second_t x =
units::meters_per_second_t{moduleStatesMatrix(i * 2, 0)};
units::meters_per_second_t y =
units::meters_per_second_t{moduleStatesMatrix(i * 2 + 1, 0)};
auto speed = units::math::hypot(x, y);
Rotation2d rotation{x.to<double>(), y.to<double>()};
moduleStates[i] = {speed, rotation};
}
return moduleStates;
}
template <size_t NumModules>
template <typename... ModuleStates>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
ModuleStates&&... wheelStates) const {
static_assert(sizeof...(wheelStates) == NumModules,
"Number of modules is not consistent with number of wheel "
"locations provided in constructor.");
std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
return this->ToChassisSpeeds(moduleStates);
}
template <size_t NumModules>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
std::array<SwerveModuleState, NumModules> moduleStates) const {
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
for (size_t i = 0; i < NumModules; i++) {
SwerveModuleState module = moduleStates[i];
moduleStatesMatrix.row(i * 2)
<< module.speed.to<double>() * module.angle.Cos();
moduleStatesMatrix.row(i * 2 + 1)
<< module.speed.to<double>() * module.angle.Sin();
}
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(moduleStatesMatrix);
return {units::meters_per_second_t{chassisSpeedsVector(0)},
units::meters_per_second_t{chassisSpeedsVector(1)},
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
template <size_t NumModules>
void SwerveDriveKinematics<NumModules>::NormalizeWheelSpeeds(
std::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed = std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return units::math::abs(a.speed) <
units::math::abs(b.speed);
})
->speed;
if (realMaxSpeed > attainableMaxSpeed) {
for (auto& module : states) {
module.speed = module.speed / realMaxSpeed * attainableMaxSpeed;
}
}
}
} // namespace frc

120
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h Normal file
View File

@@ -0,0 +1,120 @@
/*----------------------------------------------------------------------------*/
/* 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 <chrono>
#include <cstddef>
#include <ctime>
#include <wpi/timestamp.h>
#include "SwerveDriveKinematics.h"
#include "SwerveModuleState.h"
#include "frc/geometry/Pose2d.h"
#include "units/time.h"
namespace frc {
/**
* Class for swerve drive odometry. Odometry allows you to track the robot's
* position on the field over a course of a match using readings from your
* swerve drive encoders and swerve azimuth encoders.
*
* Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*/
template <size_t NumModules>
class SwerveDriveOdometry {
public:
/**
* Constructs a SwerveDriveOdometry object.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
/**
* Returns the position of the robot on the field.
* @return The pose of the robot.
*/
const Pose2d& GetPose() const { return m_pose; }
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as
* a parameter to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated
* your SwerveDriveKinematics.
*
* @return The new pose of the robot.
*/
template <typename... ModuleStates>
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates);
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates
* the current time to calculate period (difference between two timestamps).
* The period is used to calculate the change in distance from a velocity.
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated
* your SwerveDriveKinematics.
*
* @return The new pose of the robot.
*/
template <typename... ModuleStates>
const Pose2d& Update(const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, moduleStates...);
}
private:
SwerveDriveKinematics<NumModules> m_kinematics;
Pose2d m_pose;
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
};
} // namespace frc
#include "SwerveDriveOdometry.inc"

46
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc Normal file
View File

@@ -0,0 +1,46 @@
/*----------------------------------------------------------------------------*/
/* 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 "math/MathShared.h"
namespace frc {
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
}
template <size_t NumModules>
template <typename... ModuleStates>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;
auto angle = gyroAngle + m_gyroOffset;
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
static_cast<void>(dtheta);
auto newPose = m_pose.Exp(
{dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
return m_pose;
}
} // namespace frc

28
wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h Normal file
View File

@@ -0,0 +1,28 @@
/*----------------------------------------------------------------------------*/
/* 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 "frc/geometry/Rotation2d.h"
#include "units/velocity.h"
namespace frc {
/**
* Represents the state of one swerve module.
*/
struct SwerveModuleState {
/**
* Speed of the wheel of the module.
*/
units::meters_per_second_t speed = 0_mps;
/**
* Angle of the module.
*/
Rotation2d angle;
};
} // namespace frc

85
wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h Normal file
View File

@@ -0,0 +1,85 @@
/*----------------------------------------------------------------------------*/
/* 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 <array>
#include <Eigen/Core>
#include "frc/spline/Spline.h"
namespace frc {
/**
* Represents a hermite spline of degree 3.
*/
class CubicHermiteSpline : public Spline<3> {
public:
/**
* Constructs a cubic hermite spline with the specified control vectors. Each
* control vector contains info about the location of the point and its first
* derivative.
*
* @param xInitialControlVector The control vector for the initial point in
* the x dimension.
* @param xFinalControlVector The control vector for the final point in
* the x dimension.
* @param yInitialControlVector The control vector for the initial point in
* the y dimension.
* @param yFinalControlVector The control vector for the final point in
* the y dimension.
*/
CubicHermiteSpline(std::array<double, 2> xInitialControlVector,
std::array<double, 2> xFinalControlVector,
std::array<double, 2> yInitialControlVector,
std::array<double, 2> yFinalControlVector);
protected:
/**
* Returns the coefficients matrix.
* @return The coefficients matrix.
*/
Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
return m_coefficients;
}
private:
Eigen::Matrix<double, 6, 4> m_coefficients =
Eigen::Matrix<double, 6, 4>::Zero();
/**
* Returns the hermite basis matrix for cubic hermite spline interpolation.
* @return The hermite basis matrix for cubic hermite spline interpolation.
*/
static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
// clang-format off
static auto basis = (Eigen::Matrix<double, 4, 4>() <<
+2.0, +1.0, -2.0, +1.0,
-3.0, -2.0, +3.0, -1.0,
+0.0, +1.0, +0.0, +0.0,
+1.0, +0.0, +0.0, +0.0).finished();
// clang-format on
return basis;
}
/**
* Returns the control vector for each dimension as a matrix from the
* user-provided arrays in the constructor.
*
* @param initialVector The control vector for the initial point.
* @param finalVector The control vector for the final point.
*
* @return The control vector matrix for a dimension.
*/
static Eigen::Vector4d ControlVectorFromArrays(
std::array<double, 2> initialVector, std::array<double, 2> finalVector) {
return (Eigen::Vector4d() << initialVector[0], initialVector[1],
finalVector[0], finalVector[1])
.finished();
}
};
} // namespace frc

87
wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h Normal file
View File

@@ -0,0 +1,87 @@
/*----------------------------------------------------------------------------*/
/* 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 <array>
#include <Eigen/Core>
#include "frc/spline/Spline.h"
namespace frc {
/**
* Represents a hermite spline of degree 5.
*/
class QuinticHermiteSpline : public Spline<5> {
public:
/**
* Constructs a quintic hermite spline with the specified control vectors.
* Each control vector contains into about the location of the point, its
* first derivative, and its second derivative.
*
* @param xInitialControlVector The control vector for the initial point in
* the x dimension.
* @param xFinalControlVector The control vector for the final point in
* the x dimension.
* @param yInitialControlVector The control vector for the initial point in
* the y dimension.
* @param yFinalControlVector The control vector for the final point in
* the y dimension.
*/
QuinticHermiteSpline(std::array<double, 3> xInitialControlVector,
std::array<double, 3> xFinalControlVector,
std::array<double, 3> yInitialControlVector,
std::array<double, 3> yFinalControlVector);
protected:
/**
* Returns the coefficients matrix.
* @return The coefficients matrix.
*/
Eigen::Matrix<double, 6, 6> Coefficients() const override {
return m_coefficients;
}
private:
Eigen::Matrix<double, 6, 6> m_coefficients =
Eigen::Matrix<double, 6, 6>::Zero();
/**
* Returns the hermite basis matrix for quintic hermite spline interpolation.
* @return The hermite basis matrix for quintic hermite spline interpolation.
*/
static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
// clang-format off
static const auto basis = (Eigen::Matrix<double, 6, 6>() <<
-06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
+15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
-10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
+00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
+01.0, +00.0, +00.0, +00.0, +00.0, +00.0).finished();
// clang-format on
return basis;
}
/**
* Returns the control vector for each dimension as a matrix from the
* user-provided arrays in the constructor.
*
* @param initialVector The control vector for the initial point.
* @param finalVector The control vector for the final point.
*
* @return The control vector matrix for a dimension.
*/
static Eigen::Matrix<double, 6, 1> ControlVectorFromArrays(
std::array<double, 3> initialVector, std::array<double, 3> finalVector) {
return (Eigen::Matrix<double, 6, 1>() << initialVector[0], initialVector[1],
initialVector[2], finalVector[0], finalVector[1], finalVector[2])
.finished();
}
};
} // namespace frc

130
wpimath/src/main/native/include/frc/spline/Spline.h Normal file
View File

@@ -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. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <array>
#include <utility>
#include <vector>
#include <Eigen/Core>
#include "frc/geometry/Pose2d.h"
#include "units/curvature.h"
#include "units/length.h"
namespace frc {
/**
* Represents a two-dimensional parametric spline that interpolates between two
* points.
*
* @tparam Degree The degree of the spline.
*/
template <int Degree>
class Spline {
public:
using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
Spline() = default;
Spline(const Spline&) = default;
Spline& operator=(const Spline&) = default;
Spline(Spline&&) = default;
Spline& operator=(Spline&&) = default;
virtual ~Spline() = default;
/**
* Represents a control vector for a spline.
*
* Each element in each array represents the value of the derivative at the
* index. For example, the value of x[2] is the second derivative in the x
* dimension.
*/
struct ControlVector {
std::array<double, (Degree + 1) / 2> x;
std::array<double, (Degree + 1) / 2> y;
};
/**
* Gets the pose and curvature at some point t on the spline.
*
* @param t The point t
* @return The pose and curvature at that point.
*/
PoseWithCurvature GetPoint(double t) const {
Eigen::Matrix<double, Degree + 1, 1> polynomialBases;
// Populate the polynomial bases
for (int i = 0; i <= Degree; i++) {
polynomialBases(i) = std::pow(t, Degree - i);
}
// This simply multiplies by the coefficients. We need to divide out t some
// n number of times where n is the derivative we want to take.
Eigen::Matrix<double, 6, 1> combined = Coefficients() * polynomialBases;
double dx, dy, ddx, ddy;
// If t = 0, all other terms in the equation cancel out to zero. We can use
// the last x^0 term in the equation.
if (t == 0.0) {
dx = Coefficients()(2, Degree - 1);
dy = Coefficients()(3, Degree - 1);
ddx = Coefficients()(4, Degree - 2);
ddy = Coefficients()(5, Degree - 2);
} else {
// Divide out t for first derivative.
dx = combined(2) / t;
dy = combined(3) / t;
// Divide out t for second derivative.
ddx = combined(4) / t / t;
ddy = combined(5) / t / t;
}
// Find the curvature.
const auto curvature =
(dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
return {
{FromVector(combined.template block<2, 1>(0, 0)), Rotation2d(dx, dy)},
units::curvature_t(curvature)};
}
protected:
/**
* Returns the coefficients of the spline.
*
* @return The coefficients of the spline.
*/
virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
/**
* Converts a Translation2d into a vector that is compatible with Eigen.
*
* @param translation The Translation2d to convert.
* @return The vector.
*/
static Eigen::Vector2d ToVector(const Translation2d& translation) {
return (Eigen::Vector2d() << translation.X().to<double>(),
translation.Y().to<double>())
.finished();
}
/**
* Converts an Eigen vector into a Translation2d.
*
* @param vector The vector to convert.
* @return The Translation2d.
*/
static Translation2d FromVector(const Eigen::Vector2d& vector) {
return Translation2d(units::meter_t(vector(0)), units::meter_t(vector(1)));
}
};
} // namespace frc

109
wpimath/src/main/native/include/frc/spline/SplineHelper.h Normal file
View File

@@ -0,0 +1,109 @@
/*----------------------------------------------------------------------------*/
/* 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 <array>
#include <utility>
#include <vector>
#include "frc/spline/CubicHermiteSpline.h"
#include "frc/spline/QuinticHermiteSpline.h"
namespace frc {
/**
* Helper class that is used to generate cubic and quintic splines from user
* provided waypoints.
*/
class SplineHelper {
public:
/**
* Returns 2 cubic control vectors from a set of exterior waypoints and
* interior translations.
*
* @param start The starting pose.
* @param interiorWaypoints The interior waypoints.
* @param end The ending pose.
* @return 2 cubic control vectors.
*/
static std::array<Spline<3>::ControlVector, 2>
CubicControlVectorsFromWaypoints(
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end);
/**
* Returns quintic control vectors from a set of waypoints.
*
* @param waypoints The waypoints
* @return List of control vectors
*/
static std::vector<Spline<5>::ControlVector>
QuinticControlVectorsFromWaypoints(const std::vector<Pose2d>& waypoints);
/**
* Returns a set of cubic splines corresponding to the provided control
* vectors. The user is free to set the direction of the start and end
* point. The directions for the middle waypoints are determined
* automatically to ensure continuous curvature throughout the path.
*
* The derivation for the algorithm used can be found here:
* <https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08/undervisningsmateriale/chap7alecture.pdf>
*
* @param start The starting control vector.
* @param waypoints The middle waypoints. This can be left blank if you
* only wish to create a path with two waypoints.
* @param end The ending control vector.
*
* @return A vector of cubic hermite splines that interpolate through the
* provided waypoints.
*/
static std::vector<CubicHermiteSpline> CubicSplinesFromControlVectors(
const Spline<3>::ControlVector& start,
std::vector<Translation2d> waypoints,
const Spline<3>::ControlVector& end);
/**
* Returns a set of quintic splines corresponding to the provided control
* vectors. The user is free to set the direction of all waypoints. Continuous
* curvature is guaranteed throughout the path.
*
* @param controlVectors The control vectors.
* @return A vector of quintic hermite splines that interpolate through the
* provided waypoints.
*/
static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
const std::vector<Spline<5>::ControlVector>& controlVectors);
private:
static Spline<3>::ControlVector CubicControlVector(double scalar,
const Pose2d& point) {
return {{point.X().to<double>(), scalar * point.Rotation().Cos()},
{point.Y().to<double>(), scalar * point.Rotation().Sin()}};
}
static Spline<5>::ControlVector QuinticControlVector(double scalar,
const Pose2d& point) {
return {{point.X().to<double>(), scalar * point.Rotation().Cos(), 0.0},
{point.Y().to<double>(), scalar * point.Rotation().Sin(), 0.0}};
}
/**
* Thomas algorithm for solving tridiagonal systems Af = d.
*
* @param a the values of A above the diagonal
* @param b the values of A on the diagonal
* @param c the values of A below the diagonal
* @param d the vector on the rhs
* @param solutionVector the unknown (solution) vector, modified in-place
*/
static void ThomasAlgorithm(const std::vector<double>& a,
const std::vector<double>& b,
const std::vector<double>& c,
const std::vector<double>& d,
std::vector<double>* solutionVector);
};
} // namespace frc

145
wpimath/src/main/native/include/frc/spline/SplineParameterizer.h Normal file
View File

@@ -0,0 +1,145 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
/*
* MIT License
*
* Copyright (c) 2018 Team 254
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <stack>
#include <string>
#include <utility>
#include <vector>
#include <wpi/Twine.h>
#include "frc/spline/Spline.h"
#include "units/angle.h"
#include "units/curvature.h"
#include "units/length.h"
#include "units/math.h"
namespace frc {
/**
* Class used to parameterize a spline by its arc length.
*/
class SplineParameterizer {
public:
using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
struct MalformedSplineException : public std::runtime_error {
explicit MalformedSplineException(const char* what_arg)
: runtime_error(what_arg) {}
};
/**
* Parameterizes the spline. This method breaks up the spline into various
* arcs until their dx, dy, and dtheta are within specific tolerances.
*
* @param spline The spline to parameterize.
* @param t0 Starting internal spline parameter. It is recommended to leave
* this as default.
* @param t1 Ending internal spline parameter. It is recommended to leave this
* as default.
*
* @return A vector of poses and curvatures that represents various points on
* the spline.
*/
template <int Dim>
static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
double t0 = 0.0,
double t1 = 1.0) {
std::vector<PoseWithCurvature> splinePoints;
// The parameterization does not add the initial point. Let's add that.
splinePoints.push_back(spline.GetPoint(t0));
// We use an "explicit stack" to simulate recursion, instead of a recursive
// function call This give us greater control, instead of a stack overflow
std::stack<StackContents> stack;
stack.emplace(StackContents{t0, t1});
StackContents current;
PoseWithCurvature start;
PoseWithCurvature end;
int iterations = 0;
while (!stack.empty()) {
current = stack.top();
stack.pop();
start = spline.GetPoint(current.t0);
end = spline.GetPoint(current.t1);
const auto twist = start.first.Log(end.first);
if (units::math::abs(twist.dy) > kMaxDy ||
units::math::abs(twist.dx) > kMaxDx ||
units::math::abs(twist.dtheta) > kMaxDtheta) {
stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
} else {
splinePoints.push_back(spline.GetPoint(current.t1));
}
if (iterations++ >= kMaxIterations) {
throw MalformedSplineException(
"Could not parameterize a malformed spline. "
"This means that you probably had two or more adjacent "
"waypoints that were very close together with headings "
"in opposing directions.");
}
}
return splinePoints;
}
private:
// Constraints for spline parameterization.
static constexpr units::meter_t kMaxDx = 5_in;
static constexpr units::meter_t kMaxDy = 0.05_in;
static constexpr units::radian_t kMaxDtheta = 0.0872_rad;
struct StackContents {
double t0;
double t1;
};
/**
* A malformed spline does not actually explode the LIFO stack size. Instead,
* the stack size stays at a relatively small number (e.g. 30) and never
* decreases. Because of this, we must count iterations. Even long, complex
* paths don't usually go over 300 iterations, so hitting this maximum should
* definitely indicate something has gone wrong.
*/
static constexpr int kMaxIterations = 5000;
friend class CubicHermiteSplineTest;
friend class QuinticHermiteSplineTest;
};
} // namespace frc

171
wpimath/src/main/native/include/frc/trajectory/Trajectory.h Normal file
View File

@@ -0,0 +1,171 @@
/*----------------------------------------------------------------------------*/
/* 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 <vector>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Transform2d.h"
#include "units/acceleration.h"
#include "units/curvature.h"
#include "units/time.h"
#include "units/velocity.h"
namespace wpi {
class json;
} // namespace wpi
namespace frc {
/**
* Represents a time-parameterized trajectory. The trajectory contains of
* various States that represent the pose, curvature, time elapsed, velocity,
* and acceleration at that point.
*/
class Trajectory {
public:
/**
* Represents one point on the trajectory.
*/
struct State {
// The time elapsed since the beginning of the trajectory.
units::second_t t = 0_s;
// The speed at that point of the trajectory.
units::meters_per_second_t velocity = 0_mps;
// The acceleration at that point of the trajectory.
units::meters_per_second_squared_t acceleration = 0_mps_sq;
// The pose at that point of the trajectory.
Pose2d pose;
// The curvature at that point of the trajectory.
units::curvature_t curvature{0.0};
/**
* Checks equality between this State and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const State& other) const;
/**
* Checks inequality between this State and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const State& other) const;
/**
* Interpolates between two States.
*
* @param endValue The end value for the interpolation.
* @param i The interpolant (fraction).
*
* @return The interpolated state.
*/
State Interpolate(State endValue, double i) const;
};
Trajectory() = default;
/**
* Constructs a trajectory from a vector of states.
*/
explicit Trajectory(const std::vector<State>& states);
/**
* Returns the overall duration of the trajectory.
* @return The duration of the trajectory.
*/
units::second_t TotalTime() const { return m_totalTime; }
/**
* Return the states of the trajectory.
* @return The states of the trajectory.
*/
const std::vector<State>& States() const { return m_states; }
/**
* Sample the trajectory at a point in time.
*
* @param t The point in time since the beginning of the trajectory to sample.
* @return The state at that point in time.
*/
State Sample(units::second_t t) const;
/**
* Transforms all poses in the trajectory by the given transform. This is
* useful for converting a robot-relative trajectory into a field-relative
* trajectory. This works with respect to the first pose in the trajectory.
*
* @param transform The transform to transform the trajectory by.
* @return The transformed trajectory.
*/
Trajectory TransformBy(const Transform2d& transform);
/**
* Transforms all poses in the trajectory so that they are relative to the
* given pose. This is useful for converting a field-relative trajectory
* into a robot-relative trajectory.
*
* @param pose The pose that is the origin of the coordinate frame that
* the current trajectory will be transformed into.
* @return The transformed trajectory.
*/
Trajectory RelativeTo(const Pose2d& pose);
/**
* Returns the initial pose of the trajectory.
*
* @return The initial pose of the trajectory.
*/
Pose2d InitialPose() const { return Sample(0_s).pose; }
/**
* Checks equality between this Trajectory and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Trajectory& other) const;
/**
* Checks inequality between this Trajectory and another object.
*
* @param other The other object.
* @return Whether the two objects are inequal.
*/
bool operator!=(const Trajectory& other) const;
private:
std::vector<State> m_states;
units::second_t m_totalTime = 0_s;
/**
* Linearly interpolates between two values.
*
* @param startValue The start value.
* @param endValue The end value.
* @param t The fraction for interpolation.
*
* @return The interpolated value.
*/
template <typename T>
static T Lerp(const T& startValue, const T& endValue, const double t) {
return startValue + (endValue - startValue) * t;
}
};
void to_json(wpi::json& json, const Trajectory::State& state);
void from_json(const wpi::json& json, Trajectory::State& state);
} // namespace frc

165
wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h Normal file
View File

@@ -0,0 +1,165 @@
/*----------------------------------------------------------------------------*/
/* 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 <memory>
#include <utility>
#include <vector>
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
#include "frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "units/acceleration.h"
#include "units/velocity.h"
namespace frc {
/**
* Represents the configuration for generating a trajectory. This class stores
* the start velocity, end velocity, max velocity, max acceleration, custom
* constraints, and the reversed flag.
*
* The class must be constructed with a max velocity and max acceleration.
* The other parameters (start velocity, end velocity, constraints, reversed)
* have been defaulted to reasonable values (0, 0, {}, false). These values can
* be changed via the SetXXX methods.
*/
class TrajectoryConfig {
public:
/**
* Constructs a config object.
* @param maxVelocity The max velocity of the trajectory.
* @param maxAcceleration The max acceleration of the trajectory.
*/
TrajectoryConfig(units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration)
: m_maxVelocity(maxVelocity), m_maxAcceleration(maxAcceleration) {}
TrajectoryConfig(const TrajectoryConfig&) = delete;
TrajectoryConfig& operator=(const TrajectoryConfig&) = delete;
TrajectoryConfig(TrajectoryConfig&&) = default;
TrajectoryConfig& operator=(TrajectoryConfig&&) = default;
/**
* Sets the start velocity of the trajectory.
* @param startVelocity The start velocity of the trajectory.
*/
void SetStartVelocity(units::meters_per_second_t startVelocity) {
m_startVelocity = startVelocity;
}
/**
* Sets the end velocity of the trajectory.
* @param endVelocity The end velocity of the trajectory.
*/
void SetEndVelocity(units::meters_per_second_t endVelocity) {
m_endVelocity = endVelocity;
}
/**
* Sets the reversed flag of the trajectory.
* @param reversed Whether the trajectory should be reversed or not.
*/
void SetReversed(bool reversed) { m_reversed = reversed; }
/**
* Adds a user-defined constraint to the trajectory.
* @param constraint The user-defined constraint.
*/
template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
TrajectoryConstraint, Constraint>>>
void AddConstraint(Constraint constraint) {
m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
}
/**
* Adds a differential drive kinematics constraint to ensure that
* no wheel velocity of a differential drive goes above the max velocity.
*
* @param kinematics The differential drive kinematics.
*/
void SetKinematics(const DifferentialDriveKinematics& kinematics) {
AddConstraint(
DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
}
/**
* Adds a mecanum drive kinematics constraint to ensure that
* no wheel velocity of a mecanum drive goes above the max velocity.
*
* @param kinematics The mecanum drive kinematics.
*/
void SetKinematics(MecanumDriveKinematics kinematics) {
AddConstraint(MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
}
/**
* Adds a swerve drive kinematics constraint to ensure that
* no wheel velocity of a swerve drive goes above the max velocity.
*
* @param kinematics The swerve drive kinematics.
*/
template <size_t NumModules>
void SetKinematics(SwerveDriveKinematics<NumModules>& kinematics) {
AddConstraint(SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
}
/**
* Returns the starting velocity of the trajectory.
* @return The starting velocity of the trajectory.
*/
units::meters_per_second_t StartVelocity() const { return m_startVelocity; }
/**
* Returns the ending velocity of the trajectory.
* @return The ending velocity of the trajectory.
*/
units::meters_per_second_t EndVelocity() const { return m_endVelocity; }
/**
* Returns the maximum velocity of the trajectory.
* @return The maximum velocity of the trajectory.
*/
units::meters_per_second_t MaxVelocity() const { return m_maxVelocity; }
/**
* Returns the maximum acceleration of the trajectory.
* @return The maximum acceleration of the trajectory.
*/
units::meters_per_second_squared_t MaxAcceleration() const {
return m_maxAcceleration;
}
/**
* Returns the user-defined constraints of the trajectory.
* @return The user-defined constraints of the trajectory.
*/
const std::vector<std::unique_ptr<TrajectoryConstraint>>& Constraints()
const {
return m_constraints;
}
/**
* Returns whether the trajectory is reversed or not.
* @return whether the trajectory is reversed or not.
*/
bool IsReversed() const { return m_reversed; }
private:
units::meters_per_second_t m_startVelocity = 0_mps;
units::meters_per_second_t m_endVelocity = 0_mps;
units::meters_per_second_t m_maxVelocity;
units::meters_per_second_squared_t m_maxAcceleration;
std::vector<std::unique_ptr<TrajectoryConstraint>> m_constraints;
bool m_reversed = false;
};
} // namespace frc

133
wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h Normal file
View File

@@ -0,0 +1,133 @@
/*----------------------------------------------------------------------------*/
/* 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 <functional>
#include <memory>
#include <utility>
#include <vector>
#include "frc/spline/SplineParameterizer.h"
#include "frc/trajectory/Trajectory.h"
#include "frc/trajectory/TrajectoryConfig.h"
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
namespace frc {
/**
* Helper class used to generate trajectories with various constraints.
*/
class TrajectoryGenerator {
public:
using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
/**
* Generates a trajectory from the given control vectors and config. This
* method uses clamped cubic splines -- a method in which the exterior control
* vectors and interior waypoints are provided. The headings are automatically
* determined at the interior points to ensure continuous curvature.
*
* @param initial The initial control vector.
* @param interiorWaypoints The interior waypoints.
* @param end The ending control vector.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
static Trajectory GenerateTrajectory(
Spline<3>::ControlVector initial,
const std::vector<Translation2d>& interiorWaypoints,
Spline<3>::ControlVector end, const TrajectoryConfig& config);
/**
* Generates a trajectory from the given waypoints and config. This method
* uses clamped cubic splines -- a method in which the initial pose, final
* pose, and interior waypoints are provided. The headings are automatically
* determined at the interior points to ensure continuous curvature.
*
* @param start The starting pose.
* @param interiorWaypoints The interior waypoints.
* @param end The ending pose.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
static Trajectory GenerateTrajectory(
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end, const TrajectoryConfig& config);
/**
* Generates a trajectory from the given quintic control vectors and config.
* This method uses quintic hermite splines -- therefore, all points must be
* represented by control vectors. Continuous curvature is guaranteed in this
* method.
*
* @param controlVectors List of quintic control vectors.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
static Trajectory GenerateTrajectory(
std::vector<Spline<5>::ControlVector> controlVectors,
const TrajectoryConfig& config);
/**
* Generates a trajectory from the given waypoints and config. This method
* uses quintic hermite splines -- therefore, all points must be represented
* by Pose2d objects. Continuous curvature is guaranteed in this method.
*
* @param waypoints List of waypoints..
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
static Trajectory GenerateTrajectory(const std::vector<Pose2d>& waypoints,
const TrajectoryConfig& config);
/**
* Generate spline points from a vector of splines by parameterizing the
* splines.
*
* @param splines The splines to parameterize.
*
* @return The spline points for use in time parameterization of a trajectory.
*/
template <typename Spline>
static std::vector<PoseWithCurvature> SplinePointsFromSplines(
const std::vector<Spline>& splines) {
// Create the vector of spline points.
std::vector<PoseWithCurvature> splinePoints;
// Add the first point to the vector.
splinePoints.push_back(splines.front().GetPoint(0.0));
// Iterate through the vector and parameterize each spline, adding the
// parameterized points to the final vector.
for (auto&& spline : splines) {
auto points = SplineParameterizer::Parameterize(spline);
// Append the array of poses to the vector. We are removing the first
// point because it's a duplicate of the last point from the previous
// spline.
splinePoints.insert(std::end(splinePoints), std::begin(points) + 1,
std::end(points));
}
return splinePoints;
}
/**
* Set error reporting function. By default, it is output to stderr.
*
* @param func Error reporting function.
*/
static void SetErrorHandler(std::function<void(const char*)> func) {
s_errorFunc = std::move(func);
}
private:
static void ReportError(const char* error);
static const Trajectory kDoNothingTrajectory;
static std::function<void(const char*)> s_errorFunc;
};
} // namespace frc

107
wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h Normal file
View File

@@ -0,0 +1,107 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
/*
* MIT License
*
* Copyright (c) 2018 Team 254
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <memory>
#include <utility>
#include <vector>
#include "frc/trajectory/Trajectory.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
namespace frc {
/**
* Class used to parameterize a trajectory by time.
*/
class TrajectoryParameterizer {
public:
using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
/**
* Parameterize the trajectory by time. This is where the velocity profile is
* generated.
*
* The derivation of the algorithm used can be found here:
* <http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf>
*
* @param points Reference to the spline points.
* @param constraints A vector of various velocity and acceleration
* constraints.
* @param startVelocity The start velocity for the trajectory.
* @param endVelocity The end velocity for the trajectory.
* @param maxVelocity The max velocity for the trajectory.
* @param maxAcceleration The max acceleration for the trajectory.
* @param reversed Whether the robot should move backwards. Note that the
* robot will still move from a -> b -> ... -> z as defined in the waypoints.
*
* @return The trajectory.
*/
static Trajectory TimeParameterizeTrajectory(
const std::vector<PoseWithCurvature>& points,
const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
units::meters_per_second_t startVelocity,
units::meters_per_second_t endVelocity,
units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration, bool reversed);
private:
constexpr static double kEpsilon = 1E-6;
/**
* Represents a constrained state that is used when time parameterizing a
* trajectory. Each state has the pose, curvature, distance from the start of
* the trajectory, max velocity, min acceleration and max acceleration.
*/
struct ConstrainedState {
PoseWithCurvature pose = {Pose2d(), units::curvature_t(0.0)};
units::meter_t distance = 0_m;
units::meters_per_second_t maxVelocity = 0_mps;
units::meters_per_second_squared_t minAcceleration = 0_mps_sq;
units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
};
/**
* Enforces acceleration limits as defined by the constraints. This function
* is used when time parameterizing a trajectory.
*
* @param reverse Whether the robot is traveling backwards.
* @param constraints A vector of the user-defined velocity and acceleration
* constraints.
* @param state Pointer to the constrained state that we are operating on.
* This is mutated in place.
*/
static void EnforceAccelerationLimits(
bool reverse,
const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
ConstrainedState* state);
};
} // namespace frc

59
wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h Normal file
View File

@@ -0,0 +1,59 @@
/*----------------------------------------------------------------------------*/
/* 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 <string>
#include <wpi/StringRef.h>
#include <wpi/Twine.h>
#include "frc/trajectory/Trajectory.h"
namespace frc {
class TrajectoryUtil {
public:
TrajectoryUtil() = delete;
/**
* Exports a Trajectory to a PathWeaver-style JSON file.
*
* @param trajectory the trajectory to export
* @param path the path of the file to export to
*
* @return The interpolated state.
*/
static void ToPathweaverJson(const Trajectory& trajectory,
const wpi::Twine& path);
/**
* Imports a Trajectory from a PathWeaver-style JSON file.
*
* @param path The path of the json file to import from.
*
* @return The trajectory represented by the file.
*/
static Trajectory FromPathweaverJson(const wpi::Twine& path);
/**
* Deserializes a Trajectory from PathWeaver-style JSON.
* @param json the string containing the serialized JSON
* @return the trajectory represented by the JSON
*/
static std::string SerializeTrajectory(const Trajectory& trajectory);
/**
* Serializes a Trajectory to PathWeaver-style JSON.
* @param trajectory the trajectory to export
* @return the string containing the serialized JSON
*/
static Trajectory DeserializeTrajectory(wpi::StringRef json_str);
};
} // namespace frc

159
wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h Normal file
View File

@@ -0,0 +1,159 @@
/*----------------------------------------------------------------------------*/
/* 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 "math/MathShared.h"
#include "units/time.h"
namespace frc {
/**
* A trapezoid-shaped velocity profile.
*
* While this class can be used for a profiled movement from start to finish,
* the intended usage is to filter a reference's dynamics based on trapezoidal
* velocity constraints. To compute the reference obeying this constraint, do
* the following.
*
* Initialization:
* @code{.cpp}
* TrapezoidalMotionProfile::Constraints constraints{kMaxV, kMaxA};
* double previousProfiledReference = initialReference;
* @endcode
*
* Run on update:
* @code{.cpp}
* TrapezoidalMotionProfile profile{constraints, unprofiledReference,
* previousProfiledReference};
* previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
* @endcode
*
* where `unprofiledReference` is free to change between calls. Note that when
* the unprofiled reference is within the constraints, `Calculate()` returns the
* unprofiled reference unchanged.
*
* Otherwise, a timer can be started to provide monotonic values for
* `Calculate()` and to determine when the profile has completed via
* `IsFinished()`.
*/
template <class Distance>
class TrapezoidProfile {
public:
using Distance_t = units::unit_t<Distance>;
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
using Velocity_t = units::unit_t<Velocity>;
using Acceleration =
units::compound_unit<Velocity, units::inverse<units::seconds>>;
using Acceleration_t = units::unit_t<Acceleration>;
class Constraints {
public:
Constraints() {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1);
}
Constraints(Velocity_t maxVelocity_, Acceleration_t maxAcceleration_)
: maxVelocity{maxVelocity_}, maxAcceleration{maxAcceleration_} {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1);
}
Velocity_t maxVelocity{0};
Acceleration_t maxAcceleration{0};
};
class State {
public:
Distance_t position{0};
Velocity_t velocity{0};
bool operator==(const State& rhs) const {
return position == rhs.position && velocity == rhs.velocity;
}
bool operator!=(const State& rhs) const { return !(*this == rhs); }
};
/**
* Construct a TrapezoidProfile.
*
* @param constraints The constraints on the profile, like maximum velocity.
* @param goal The desired state when the profile is complete.
* @param initial The initial state (usually the current state).
*/
TrapezoidProfile(Constraints constraints, State goal,
State initial = State{Distance_t(0), Velocity_t(0)});
TrapezoidProfile(const TrapezoidProfile&) = default;
TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
TrapezoidProfile(TrapezoidProfile&&) = default;
TrapezoidProfile& operator=(TrapezoidProfile&&) = default;
/**
* Calculate the correct position and velocity for the profile at a time t
* where the beginning of the profile was at time t = 0.
*
* @param t The time since the beginning of the profile.
*/
State Calculate(units::second_t t) const;
/**
* Returns the time left until a target distance in the profile is reached.
*
* @param target The target distance.
*/
units::second_t TimeLeftUntil(Distance_t target) const;
/**
* Returns the total time the profile takes to reach the goal.
*/
units::second_t TotalTime() const { return m_endDeccel; }
/**
* Returns true if the profile has reached the goal.
*
* The profile has reached the goal if the time since the profile started
* has exceeded the profile's total time.
*
* @param t The time since the beginning of the profile.
*/
bool IsFinished(units::second_t t) const { return t >= TotalTime(); }
private:
/**
* Returns true if the profile inverted.
*
* The profile is inverted if goal position is less than the initial position.
*
* @param initial The initial state (usually the current state).
* @param goal The desired state when the profile is complete.
*/
static bool ShouldFlipAcceleration(const State& initial, const State& goal) {
return initial.position > goal.position;
}
// Flip the sign of the velocity and position if the profile is inverted
State Direct(const State& in) const {
State result = in;
result.position *= m_direction;
result.velocity *= m_direction;
return result;
}
// The direction of the profile, either 1 for forwards or -1 for inverted
int m_direction;
Constraints m_constraints;
State m_initial;
State m_goal;
units::second_t m_endAccel;
units::second_t m_endFullSpeed;
units::second_t m_endDeccel;
};
} // namespace frc
#include "TrapezoidProfile.inc"

165
wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc Normal file
View File

@@ -0,0 +1,165 @@
/*----------------------------------------------------------------------------*/
/* 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 <algorithm>
#include "units/math.h"
namespace frc {
template <class Distance>
TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
State goal, State initial)
: m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
m_constraints(constraints),
m_initial(Direct(initial)),
m_goal(Direct(goal)) {
if (m_initial.velocity > m_constraints.maxVelocity) {
m_initial.velocity = m_constraints.maxVelocity;
}
// Deal with a possibly truncated motion profile (with nonzero initial or
// final velocity) by calculating the parameters as if the profile began and
// ended at zero velocity
units::second_t cutoffBegin =
m_initial.velocity / m_constraints.maxAcceleration;
Distance_t cutoffDistBegin =
cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
Distance_t cutoffDistEnd =
cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
// Now we can calculate the parameters as if it was a full trapezoid instead
// of a truncated one
Distance_t fullTrapezoidDist =
cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
units::second_t accelerationTime =
m_constraints.maxVelocity / m_constraints.maxAcceleration;
Distance_t fullSpeedDist =
fullTrapezoidDist -
accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
if (fullSpeedDist < Distance_t(0)) {
accelerationTime =
units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
fullSpeedDist = Distance_t(0);
}
m_endAccel = accelerationTime - cutoffBegin;
m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
}
template <class Distance>
typename TrapezoidProfile<Distance>::State
TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
State result = m_initial;
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
result.position +=
(m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
result.velocity = m_constraints.maxVelocity;
result.position += (m_initial.velocity +
m_endAccel * m_constraints.maxAcceleration / 2.0) *
m_endAccel +
m_constraints.maxVelocity * (t - m_endAccel);
} else if (t <= m_endDeccel) {
result.velocity =
m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
units::second_t timeLeft = m_endDeccel - t;
result.position =
m_goal.position -
(m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
timeLeft;
} else {
result = m_goal;
}
return Direct(result);
}
template <class Distance>
units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
Distance_t target) const {
Distance_t position = m_initial.position * m_direction;
Velocity_t velocity = m_initial.velocity * m_direction;
units::second_t endAccel = m_endAccel * m_direction;
units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
if (target < position) {
endAccel *= -1.0;
endFullSpeed *= -1.0;
velocity *= -1.0;
}
endAccel = units::math::max(endAccel, 0_s);
endFullSpeed = units::math::max(endFullSpeed, 0_s);
units::second_t endDeccel = m_endDeccel - endAccel - endFullSpeed;
endDeccel = units::math::max(endDeccel, 0_s);
const Acceleration_t acceleration = m_constraints.maxAcceleration;
const Acceleration_t decceleration = -m_constraints.maxAcceleration;
Distance_t distToTarget = units::math::abs(target - position);
if (distToTarget < Distance_t(1e-6)) {
return 0_s;
}
Distance_t accelDist =
velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
Velocity_t deccelVelocity;
if (endAccel > 0_s) {
deccelVelocity = units::math::sqrt(
units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
} else {
deccelVelocity = velocity;
}
Distance_t deccelDist =
deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
deccelDist = units::math::max(deccelDist, Distance_t(0));
Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
if (accelDist > distToTarget) {
accelDist = distToTarget;
fullSpeedDist = Distance_t(0);
deccelDist = Distance_t(0);
} else if (accelDist + fullSpeedDist > distToTarget) {
fullSpeedDist = distToTarget - accelDist;
deccelDist = Distance_t(0);
} else {
deccelDist = distToTarget - fullSpeedDist - accelDist;
}
units::second_t accelTime =
(-velocity + units::math::sqrt(units::math::abs(
velocity * velocity + 2 * acceleration * accelDist))) /
acceleration;
units::second_t deccelTime =
(-deccelVelocity +
units::math::sqrt(units::math::abs(deccelVelocity * deccelVelocity +
2 * decceleration * deccelDist))) /
decceleration;
units::second_t fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
return accelTime + fullSpeedTime + deccelTime;
}
} // namespace frc

41
wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h Normal file
View File

@@ -0,0 +1,41 @@
/*----------------------------------------------------------------------------*/
/* 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 "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "units/acceleration.h"
#include "units/curvature.h"
#include "units/velocity.h"
namespace frc {
/**
* A constraint on the maximum absolute centripetal acceleration allowed when
* traversing a trajectory. The centripetal acceleration of a robot is defined
* as the velocity squared divided by the radius of curvature.
*
* Effectively, limiting the maximum centripetal acceleration will cause the
* robot to slow down around tight turns, making it easier to track trajectories
* with sharp turns.
*/
class CentripetalAccelerationConstraint : public TrajectoryConstraint {
public:
explicit CentripetalAccelerationConstraint(
units::meters_per_second_squared_t maxCentripetalAcceleration);
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override;
private:
units::meters_per_second_squared_t m_maxCentripetalAcceleration;
};
} // namespace frc

37
wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h Normal file
View File

@@ -0,0 +1,37 @@
/*----------------------------------------------------------------------------*/
/* 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 "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "units/velocity.h"
namespace frc {
/**
* A class that enforces constraints on the differential drive kinematics.
* This can be used to ensure that the trajectory is constructed so that the
* commanded velocities for both sides of the drivetrain stay below a certain
* limit.
*/
class DifferentialDriveKinematicsConstraint : public TrajectoryConstraint {
public:
DifferentialDriveKinematicsConstraint(DifferentialDriveKinematics kinematics,
units::meters_per_second_t maxSpeed);
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override;
private:
DifferentialDriveKinematics m_kinematics;
units::meters_per_second_t m_maxSpeed;
};
} // namespace frc

51
wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h Normal file
View File

@@ -0,0 +1,51 @@
/*----------------------------------------------------------------------------*/
/* 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 "frc/controller/SimpleMotorFeedforward.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "units/length.h"
#include "units/voltage.h"
namespace frc {
/**
* A class that enforces constraints on differential drive voltage expenditure
* based on the motor dynamics and the drive kinematics. Ensures that the
* acceleration of any wheel of the robot while following the trajectory is
* never higher than what can be achieved with the given maximum voltage.
*/
class DifferentialDriveVoltageConstraint : public TrajectoryConstraint {
public:
/**
* Creates a new DifferentialDriveVoltageConstraint.
*
* @param feedforward A feedforward component describing the behavior of the
* drive.
* @param kinematics A kinematics component describing the drive geometry.
* @param maxVoltage The maximum voltage available to the motors while
* following the path. Should be somewhat less than the nominal battery
* voltage (12V) to account for "voltage sag" due to current draw.
*/
DifferentialDriveVoltageConstraint(
SimpleMotorFeedforward<units::meter> feedforward,
DifferentialDriveKinematics kinematics, units::volt_t maxVoltage);
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override;
private:
SimpleMotorFeedforward<units::meter> m_feedforward;
DifferentialDriveKinematics m_kinematics;
units::volt_t m_maxVoltage;
};
} // namespace frc

56
wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h Normal file
View File

@@ -0,0 +1,56 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 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 "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "units/length.h"
namespace frc {
/**
* Enforces a particular constraint only within an elliptical region.
*/
class EllipticalRegionConstraint : public TrajectoryConstraint {
public:
/**
* Constructs a new EllipticalRegionConstraint.
*
* @param center The center of the ellipse in which to enforce the constraint.
* @param xWidth The width of the ellipse in which to enforce the constraint.
* @param yWidth The height of the ellipse in which to enforce the constraint.
* @param rotation The rotation to apply to all radii around the origin.
* @param constraint The constraint to enforce when the robot is within the
* region.
*/
EllipticalRegionConstraint(const Translation2d& center, units::meter_t xWidth,
units::meter_t yWidth, const Rotation2d& rotation,
const TrajectoryConstraint& constraint);
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override;
/**
* Returns whether the specified robot pose is within the region that the
* constraint is enforced in.
*
* @param pose The robot pose.
* @return Whether the robot pose is within the constraint region.
*/
bool IsPoseInRegion(const Pose2d& pose) const;
private:
Translation2d m_center;
Translation2d m_radii;
const TrajectoryConstraint& m_constraint;
};
} // namespace frc

44
wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h Normal file
View File

@@ -0,0 +1,44 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 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 "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "units/math.h"
#include "units/velocity.h"
namespace frc {
/**
* Represents a constraint that enforces a max velocity. This can be composed
* with the EllipticalRegionConstraint or RectangularRegionConstraint to enforce
* a max velocity within a region.
*/
class MaxVelocityConstraint : public TrajectoryConstraint {
public:
/**
* Constructs a new MaxVelocityConstraint.
*
* @param maxVelocity The max velocity.
*/
explicit MaxVelocityConstraint(units::meters_per_second_t maxVelocity)
: m_maxVelocity(units::math::abs(maxVelocity)) {}
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override {
return m_maxVelocity;
}
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override {
return {};
}
private:
units::meters_per_second_t m_maxVelocity;
};
} // namespace frc

39
wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h Normal file
View File

@@ -0,0 +1,39 @@
/*----------------------------------------------------------------------------*/
/* 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 <cmath>
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "units/velocity.h"
namespace frc {
/**
* A class that enforces constraints on the mecanum drive kinematics.
* This can be used to ensure that the trajectory is constructed so that the
* commanded velocities for wheels of the drivetrain stay below a certain
* limit.
*/
class MecanumDriveKinematicsConstraint : public TrajectoryConstraint {
public:
MecanumDriveKinematicsConstraint(MecanumDriveKinematics kinematics,
units::meters_per_second_t maxSpeed);
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override;
private:
MecanumDriveKinematics m_kinematics;
units::meters_per_second_t m_maxSpeed;
};
} // namespace frc

55
wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h Normal file
View File

@@ -0,0 +1,55 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 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 "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
namespace frc {
/**
* Enforces a particular constraint only within a rectangular region.
*/
class RectangularRegionConstraint : public TrajectoryConstraint {
public:
/**
* Constructs a new RectangularRegionConstraint.
*
* @param bottomLeftPoint The bottom left point of the rectangular region in
* which to enforce the constraint.
* @param topRightPoint The top right point of the rectangular region in which
* to enforce the constraint.
* @param constraint The constraint to enforce when the robot is within the
* region.
*/
RectangularRegionConstraint(const Translation2d& bottomLeftPoint,
const Translation2d& topRightPoint,
const TrajectoryConstraint& constraint);
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override;
/**
* Returns whether the specified robot pose is within the region that the
* constraint is enforced in.
*
* @param pose The robot pose.
* @return Whether the robot pose is within the constraint region.
*/
bool IsPoseInRegion(const Pose2d& pose) const;
private:
Translation2d m_bottomLeftPoint;
Translation2d m_topRightPoint;
const TrajectoryConstraint& m_constraint;
};
} // namespace frc

44
wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h Normal file
View File

@@ -0,0 +1,44 @@
/*----------------------------------------------------------------------------*/
/* 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 <cmath>
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "units/velocity.h"
namespace frc {
template <size_t NumModules>
/**
* A class that enforces constraints on the swerve drive kinematics.
* This can be used to ensure that the trajectory is constructed so that the
* commanded velocities of the wheels stay below a certain limit.
*/
class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
public:
SwerveDriveKinematicsConstraint(
frc::SwerveDriveKinematics<NumModules> kinematics,
units::meters_per_second_t maxSpeed);
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override;
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override;
private:
frc::SwerveDriveKinematics<NumModules> m_kinematics;
units::meters_per_second_t m_maxSpeed;
};
} // namespace frc
#include "SwerveDriveKinematicsConstraint.inc"

50
wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc Normal file
View File

@@ -0,0 +1,50 @@
/*----------------------------------------------------------------------------*/
/* 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/math.h>
namespace frc {
template <size_t NumModules>
/**
* A class that enforces constraints on the swerve drive kinematics.
* This can be used to ensure that the trajectory is constructed so that the
* commanded velocities of the wheels stay below a certain limit.
*/
SwerveDriveKinematicsConstraint<NumModules>::SwerveDriveKinematicsConstraint(
frc::SwerveDriveKinematics<NumModules> kinematics,
units::meters_per_second_t maxSpeed)
: m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
template <size_t NumModules>
units::meters_per_second_t
SwerveDriveKinematicsConstraint<NumModules>::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
auto xVelocity = velocity * pose.Rotation().Cos();
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
{xVelocity, yVelocity, velocity * curvature});
m_kinematics.NormalizeWheelSpeeds(&wheelSpeeds, m_maxSpeed);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
}
template <size_t NumModules>
TrajectoryConstraint::MinMax
SwerveDriveKinematicsConstraint<NumModules>::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
return {};
}
} // namespace frc

80
wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h Normal file
View File

@@ -0,0 +1,80 @@
/*----------------------------------------------------------------------------*/
/* 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 <limits>
#include "frc/geometry/Pose2d.h"
#include "frc/spline/Spline.h"
#include "units/acceleration.h"
#include "units/curvature.h"
#include "units/velocity.h"
namespace frc {
/**
* An interface for defining user-defined velocity and acceleration constraints
* while generating trajectories.
*/
class TrajectoryConstraint {
public:
TrajectoryConstraint() = default;
TrajectoryConstraint(const TrajectoryConstraint&) = default;
TrajectoryConstraint& operator=(const TrajectoryConstraint&) = default;
TrajectoryConstraint(TrajectoryConstraint&&) = default;
TrajectoryConstraint& operator=(TrajectoryConstraint&&) = default;
virtual ~TrajectoryConstraint() = default;
/**
* Represents a minimum and maximum acceleration.
*/
struct MinMax {
/**
* The minimum acceleration.
*/
units::meters_per_second_squared_t minAcceleration{
-std::numeric_limits<double>::max()};
/**
* The maximum acceleration.
*/
units::meters_per_second_squared_t maxAcceleration{
std::numeric_limits<double>::max()};
};
/**
* Returns the max velocity given the current pose and curvature.
*
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory.
* @param velocity The velocity at the current point in the trajectory before
* constraints are applied.
*
* @return The absolute maximum velocity.
*/
virtual units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const = 0;
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
* given pose, curvature, and speed.
*
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory.
* @param speed The speed at the current point in the trajectory.
*
* @return The min and max acceleration bounds.
*/
virtual MinMax MinMaxAcceleration(const Pose2d& pose,
units::curvature_t curvature,
units::meters_per_second_t speed) const = 0;
};
} // namespace frc