Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01: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

110
wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
View File

@@ -1,110 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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. */
/*----------------------------------------------------------------------------*/
#include "frc/geometry/Pose2d.h"
#include <cmath>
#include <wpi/json.h>
using namespace frc;
Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
: m_translation(translation), m_rotation(rotation) {}
Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
: m_translation(x, y), m_rotation(rotation) {}
Pose2d Pose2d::operator+(const Transform2d& other) const {
return TransformBy(other);
}
Pose2d& Pose2d::operator+=(const Transform2d& other) {
m_translation += other.Translation().RotateBy(m_rotation);
m_rotation += other.Rotation();
return *this;
}
Transform2d Pose2d::operator-(const Pose2d& other) const {
const auto pose = this->RelativeTo(other);
return Transform2d(pose.Translation(), pose.Rotation());
}
bool Pose2d::operator==(const Pose2d& other) const {
return m_translation == other.m_translation && m_rotation == other.m_rotation;
}
bool Pose2d::operator!=(const Pose2d& other) const {
return !operator==(other);
}
Pose2d Pose2d::TransformBy(const Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
m_rotation + other.Rotation()};
}
Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
const Transform2d transform{other, *this};
return {transform.Translation(), transform.Rotation()};
}
Pose2d Pose2d::Exp(const Twist2d& twist) const {
const auto dx = twist.dx;
const auto dy = twist.dy;
const auto dtheta = twist.dtheta.to<double>();
const auto sinTheta = std::sin(dtheta);
const auto cosTheta = std::cos(dtheta);
double s, c;
if (std::abs(dtheta) < 1E-9) {
s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
c = 0.5 * dtheta;
} else {
s = sinTheta / dtheta;
c = (1 - cosTheta) / dtheta;
}
const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s},
Rotation2d{cosTheta, sinTheta}};
return *this + transform;
}
Twist2d Pose2d::Log(const Pose2d& end) const {
const auto transform = end.RelativeTo(*this);
const auto dtheta = transform.Rotation().Radians().to<double>();
const auto halfDtheta = dtheta / 2.0;
const auto cosMinusOne = transform.Rotation().Cos() - 1;
double halfThetaByTanOfHalfDtheta;
if (std::abs(cosMinusOne) < 1E-9) {
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halfThetaByTanOfHalfDtheta =
-(halfDtheta * transform.Rotation().Sin()) / cosMinusOne;
}
const Translation2d translationPart =
transform.Translation().RotateBy(
{halfThetaByTanOfHalfDtheta, -halfDtheta}) *
std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)};
}
void frc::to_json(wpi::json& json, const Pose2d& pose) {
json = wpi::json{{"translation", pose.Translation()},
{"rotation", pose.Rotation()}};
}
void frc::from_json(const wpi::json& json, Pose2d& pose) {
pose = Pose2d{json.at("translation").get<Translation2d>(),
json.at("rotation").get<Rotation2d>()};
}

86
wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
View File

@@ -1,86 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/geometry/Rotation2d.h"
#include <cmath>
#include <units/math.h>
#include <wpi/json.h>
using namespace frc;
Rotation2d::Rotation2d(units::radian_t value)
: m_value(value),
m_cos(units::math::cos(value)),
m_sin(units::math::sin(value)) {}
Rotation2d::Rotation2d(units::degree_t value)
: m_value(value),
m_cos(units::math::cos(value)),
m_sin(units::math::sin(value)) {}
Rotation2d::Rotation2d(double x, double y) {
const auto magnitude = std::hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
}
m_value = units::radian_t(std::atan2(m_sin, m_cos));
}
Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
return RotateBy(other);
}
Rotation2d& Rotation2d::operator+=(const Rotation2d& other) {
double cos = Cos() * other.Cos() - Sin() * other.Sin();
double sin = Cos() * other.Sin() + Sin() * other.Cos();
m_cos = cos;
m_sin = sin;
m_value = units::radian_t(std::atan2(m_sin, m_cos));
return *this;
}
Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
return *this + -other;
}
Rotation2d& Rotation2d::operator-=(const Rotation2d& other) {
*this += -other;
return *this;
}
Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); }
Rotation2d Rotation2d::operator*(double scalar) const {
return Rotation2d(m_value * scalar);
}
bool Rotation2d::operator==(const Rotation2d& other) const {
return units::math::abs(m_value - other.m_value) < 1E-9_rad;
}
bool Rotation2d::operator!=(const Rotation2d& other) const {
return !operator==(other);
}
Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
return {Cos() * other.Cos() - Sin() * other.Sin(),
Cos() * other.Sin() + Sin() * other.Cos()};
}
void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
json = wpi::json{{"radians", rotation.Radians().to<double>()}};
}
void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
}

40
wpilibc/src/main/native/cpp/geometry/Transform2d.cpp
View File

@@ -1,40 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/geometry/Transform2d.h"
#include "frc/geometry/Pose2d.h"
using namespace frc;
Transform2d::Transform2d(Pose2d initial, Pose2d final) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation = (final.Translation() - initial.Translation())
.RotateBy(-initial.Rotation());
m_rotation = final.Rotation() - initial.Rotation();
}
Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
: m_translation(translation), m_rotation(rotation) {}
Transform2d Transform2d::Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
bool Transform2d::operator==(const Transform2d& other) const {
return m_translation == other.m_translation && m_rotation == other.m_rotation;
}
bool Transform2d::operator!=(const Transform2d& other) const {
return !operator==(other);
}

88
wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -1,88 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/geometry/Translation2d.h"
#include <units/math.h>
#include <wpi/json.h>
using namespace frc;
Translation2d::Translation2d(units::meter_t x, units::meter_t y)
: m_x(x), m_y(y) {}
units::meter_t Translation2d::Distance(const Translation2d& other) const {
return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
}
units::meter_t Translation2d::Norm() const {
return units::math::hypot(m_x, m_y);
}
Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
return {m_x * other.Cos() - m_y * other.Sin(),
m_x * other.Sin() + m_y * other.Cos()};
}
Translation2d Translation2d::operator+(const Translation2d& other) const {
return {X() + other.X(), Y() + other.Y()};
}
Translation2d& Translation2d::operator+=(const Translation2d& other) {
m_x += other.m_x;
m_y += other.m_y;
return *this;
}
Translation2d Translation2d::operator-(const Translation2d& other) const {
return *this + -other;
}
Translation2d& Translation2d::operator-=(const Translation2d& other) {
*this += -other;
return *this;
}
Translation2d Translation2d::operator-() const { return {-m_x, -m_y}; }
Translation2d Translation2d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y};
}
Translation2d& Translation2d::operator*=(double scalar) {
m_x *= scalar;
m_y *= scalar;
return *this;
}
Translation2d Translation2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
bool Translation2d::operator==(const Translation2d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m;
}
bool Translation2d::operator!=(const Translation2d& other) const {
return !operator==(other);
}
Translation2d& Translation2d::operator/=(double scalar) {
*this *= (1.0 / scalar);
return *this;
}
void frc::to_json(wpi::json& json, const Translation2d& translation) {
json = wpi::json{{"x", translation.X().to<double>()},
{"y", translation.Y().to<double>()}};
}
void frc::from_json(const wpi::json& json, Translation2d& translation) {
translation = Translation2d{units::meter_t{json.at("x").get<double>()},
units::meter_t{json.at("y").get<double>()}};
}

42
wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
View File

@@ -1,42 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/DifferentialDriveOdometry.h"
#include <hal/FRCUsageReporting.h>
using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
const Rotation2d& gyroAngle, const Pose2d& initialPose)
: m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
HAL_Report(HALUsageReporting::kResourceType_Odometry,
HALUsageReporting::kOdometry_DifferentialDrive);
}
const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance) {
auto deltaLeftDistance = leftDistance - m_prevLeftDistance;
auto deltaRightDistance = rightDistance - m_prevRightDistance;
m_prevLeftDistance = leftDistance;
m_prevRightDistance = rightDistance;
auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
auto angle = gyroAngle + m_gyroOffset;
auto newPose = m_pose.Exp(
{averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
return m_pose;
}

23
wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
View File

@@ -1,23 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include <units/math.h>
using namespace frc;
void DifferentialDriveWheelSpeeds::Normalize(
units::meters_per_second_t attainableMaxSpeed) {
auto realMaxSpeed =
units::math::max(units::math::abs(left), units::math::abs(right));
if (realMaxSpeed > attainableMaxSpeed) {
left = left / realMaxSpeed * attainableMaxSpeed;
right = right / realMaxSpeed * attainableMaxSpeed;
}
}

69
wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -1,69 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/MecanumDriveKinematics.h"
using namespace frc;
MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
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) {
auto fl = m_frontLeftWheel - centerOfRotation;
auto fr = m_frontRightWheel - centerOfRotation;
auto rl = m_rearLeftWheel - centerOfRotation;
auto rr = m_rearRightWheel - centerOfRotation;
SetInverseKinematics(fl, fr, rl, rr);
m_previousCoR = centerOfRotation;
}
Eigen::Vector3d chassisSpeedsVector;
chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
Eigen::Matrix<double, 4, 1> wheelsMatrix =
m_inverseKinematics * chassisSpeedsVector;
MecanumDriveWheelSpeeds wheelSpeeds;
wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)};
wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)};
wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)};
wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)};
return wheelSpeeds;
}
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const {
Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
// clang-format off
wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>();
// clang-format on
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(wheelSpeedsMatrix);
return {units::meters_per_second_t{chassisSpeedsVector(0)},
units::meters_per_second_t{chassisSpeedsVector(1)},
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
Translation2d fr,
Translation2d rl,
Translation2d rr) const {
// clang-format off
m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
1, 1, (fr.X() - fr.Y()).template to<double>(),
1, 1, (rl.X() - rl.Y()).template to<double>(),
1, -1, (-(rr.X() + rr.Y())).template to<double>();
// clang-format on
m_inverseKinematics /= std::sqrt(2);
}

43
wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
View File

@@ -1,43 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/MecanumDriveOdometry.h"
#include <hal/FRCUsageReporting.h>
using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
HAL_Report(HALUsageReporting::kResourceType_Odometry,
HALUsageReporting::kOdometry_MecanumDrive);
}
const Pose2d& MecanumDriveOdometry::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
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(wheelSpeeds);
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;
}

36
wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
View File

@@ -1,36 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include <algorithm>
#include <array>
#include <cmath>
#include <units/math.h>
using namespace frc;
void MecanumDriveWheelSpeeds::Normalize(
units::meters_per_second_t attainableMaxSpeed) {
std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
rearLeft, rearRight};
units::meters_per_second_t realMaxSpeed = *std::max_element(
wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
return units::math::abs(a) < units::math::abs(b);
});
if (realMaxSpeed > attainableMaxSpeed) {
for (int i = 0; i < 4; ++i) {
wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
}
frontLeft = wheelSpeeds[0];
frontRight = wheelSpeeds[1];
rearLeft = wheelSpeeds[2];
rearRight = wheelSpeeds[3];
}
}

46
wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
View File

@@ -1,46 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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. */
/*----------------------------------------------------------------------------*/
#include "frc/spline/CubicHermiteSpline.h"
using namespace frc;
CubicHermiteSpline::CubicHermiteSpline(
std::array<double, 2> xInitialControlVector,
std::array<double, 2> xFinalControlVector,
std::array<double, 2> yInitialControlVector,
std::array<double, 2> yFinalControlVector) {
const auto hermite = MakeHermiteBasis();
const auto x =
ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
const auto y =
ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
// Populate first two rows with coefficients.
m_coefficients.template block<1, 4>(0, 0) = hermite * x;
m_coefficients.template block<1, 4>(1, 0) = hermite * y;
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
for (int i = 0; i < 4; i++) {
// Here, we are multiplying by (3 - i) to manually take the derivative. The
// power of the term in index 0 is 3, index 1 is 2 and so on. To find the
// coefficient of the derivative, we can use the power rule and multiply
// the existing coefficient by its power.
m_coefficients.template block<2, 1>(2, i) =
m_coefficients.template block<2, 1>(0, i) * (3 - i);
}
for (int i = 0; i < 3; i++) {
// Here, we are multiplying by (2 - i) to manually take the derivative. The
// power of the term in index 0 is 2, index 1 is 1 and so on. To find the
// coefficient of the derivative, we can use the power rule and multiply
// the existing coefficient by its power.
m_coefficients.template block<2, 1>(4, i) =
m_coefficients.template block<2, 1>(2, i) * (2 - i);
}
}

45
wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
View File

@@ -1,45 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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. */
/*----------------------------------------------------------------------------*/
#include "frc/spline/QuinticHermiteSpline.h"
using namespace frc;
QuinticHermiteSpline::QuinticHermiteSpline(
std::array<double, 3> xInitialControlVector,
std::array<double, 3> xFinalControlVector,
std::array<double, 3> yInitialControlVector,
std::array<double, 3> yFinalControlVector) {
const auto hermite = MakeHermiteBasis();
const auto x =
ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
const auto y =
ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
// Populate first two rows with coefficients.
m_coefficients.template block<1, 6>(0, 0) = (hermite * x).transpose();
m_coefficients.template block<1, 6>(1, 0) = (hermite * y).transpose();
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
for (int i = 0; i < 6; i++) {
// Here, we are multiplying by (5 - i) to manually take the derivative. The
// power of the term in index 0 is 5, index 1 is 4 and so on. To find the
// coefficient of the derivative, we can use the power rule and multiply
// the existing coefficient by its power.
m_coefficients.template block<2, 1>(2, i) =
m_coefficients.template block<2, 1>(0, i) * (5 - i);
}
for (int i = 0; i < 5; i++) {
// Here, we are multiplying by (4 - i) to manually take the derivative. The
// power of the term in index 0 is 4, index 1 is 3 and so on. To find the
// coefficient of the derivative, we can use the power rule and multiply
// the existing coefficient by its power.
m_coefficients.template block<2, 1>(4, i) =
m_coefficients.template block<2, 1>(2, i) * (4 - i);
}
}

214
wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
View File

@@ -1,214 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/spline/SplineHelper.h"
#include <cstddef>
using namespace frc;
std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
const Spline<3>::ControlVector& start, std::vector<Translation2d> waypoints,
const Spline<3>::ControlVector& end) {
std::vector<CubicHermiteSpline> splines;
std::array<double, 2> xInitial = start.x;
std::array<double, 2> yInitial = start.y;
std::array<double, 2> xFinal = end.x;
std::array<double, 2> yFinal = end.y;
if (waypoints.size() > 1) {
waypoints.emplace(waypoints.begin(),
Translation2d{units::meter_t(xInitial[0]),
units::meter_t(yInitial[0])});
waypoints.emplace_back(
Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])});
// Populate tridiagonal system for clamped cubic
/* See:
https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
/undervisningsmateriale/chap7alecture.pdf
*/
// Above-diagonal of tridiagonal matrix, zero-padded
std::vector<double> a;
// Diagonal of tridiagonal matrix
std::vector<double> b(waypoints.size() - 2, 4.0);
// Below-diagonal of tridiagonal matrix, zero-padded
std::vector<double> c;
// rhs vectors
std::vector<double> dx, dy;
// solution vectors
std::vector<double> fx(waypoints.size() - 2, 0.0),
fy(waypoints.size() - 2, 0.0);
// populate above-diagonal and below-diagonal vectors
a.emplace_back(0);
for (size_t i = 0; i < waypoints.size() - 3; ++i) {
a.emplace_back(1);
c.emplace_back(1);
}
c.emplace_back(0);
// populate rhs vectors
dx.emplace_back(
3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
xInitial[1]);
dy.emplace_back(
3 * (waypoints[2].Y().to<double>() - waypoints[0].Y().to<double>()) -
yInitial[1]);
if (waypoints.size() > 4) {
for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
// dx and dy represent the derivatives of the internal waypoints. The
// derivative of the second internal waypoint should involve the third
// and first internal waypoint, which have indices of 1 and 3 in the
// waypoints list (which contains ALL waypoints).
dx.emplace_back(3 * (waypoints[i + 2].X().to<double>() -
waypoints[i].X().to<double>()));
dy.emplace_back(3 * (waypoints[i + 2].Y().to<double>() -
waypoints[i].Y().to<double>()));
}
}
dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -
waypoints[waypoints.size() - 3].X().to<double>()) -
xFinal[1]);
dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to<double>() -
waypoints[waypoints.size() - 3].Y().to<double>()) -
yFinal[1]);
// Compute solution to tridiagonal system
ThomasAlgorithm(a, b, c, dx, &fx);
ThomasAlgorithm(a, b, c, dy, &fy);
fx.emplace(fx.begin(), xInitial[1]);
fx.emplace_back(xFinal[1]);
fy.emplace(fy.begin(), yInitial[1]);
fy.emplace_back(yFinal[1]);
for (size_t i = 0; i < fx.size() - 1; ++i) {
// Create the spline.
const CubicHermiteSpline spline{
{waypoints[i].X().to<double>(), fx[i]},
{waypoints[i + 1].X().to<double>(), fx[i + 1]},
{waypoints[i].Y().to<double>(), fy[i]},
{waypoints[i + 1].Y().to<double>(), fy[i + 1]}};
splines.push_back(spline);
}
} else if (waypoints.size() == 1) {
const double xDeriv =
(3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
const double yDeriv =
(3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
std::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
xDeriv};
std::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
yDeriv};
splines.emplace_back(xInitial, midXControlVector, yInitial,
midYControlVector);
splines.emplace_back(midXControlVector, xFinal, midYControlVector, yFinal);
} else {
// Create the spline.
const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal};
splines.push_back(spline);
}
return splines;
}
std::vector<QuinticHermiteSpline>
SplineHelper::QuinticSplinesFromControlVectors(
const std::vector<Spline<5>::ControlVector>& controlVectors) {
std::vector<QuinticHermiteSpline> splines;
// There are twice as many control vectors are there are splines,
// so we increment the counter by 2.
for (size_t i = 0; i < controlVectors.size() - 1; i += 2) {
auto& xInitial = controlVectors[i].x;
auto& yInitial = controlVectors[i].y;
auto& xFinal = controlVectors[i + 1].x;
auto& yFinal = controlVectors[i + 1].y;
splines.emplace_back(xInitial, xFinal, yInitial, yFinal);
}
return splines;
}
std::array<Spline<3>::ControlVector, 2>
SplineHelper::CubicControlVectorsFromWaypoints(
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end) {
double scalar;
if (interiorWaypoints.empty()) {
scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
} else {
scalar =
1.2 *
start.Translation().Distance(interiorWaypoints.front()).to<double>();
}
const auto initialCV = CubicControlVector(scalar, start);
if (!interiorWaypoints.empty()) {
scalar =
1.2 * end.Translation().Distance(interiorWaypoints.back()).to<double>();
}
const auto finalCV = CubicControlVector(scalar, end);
return {initialCV, finalCV};
}
std::vector<Spline<5>::ControlVector>
SplineHelper::QuinticControlVectorsFromWaypoints(
const std::vector<Pose2d>& waypoints) {
std::vector<Spline<5>::ControlVector> vectors;
for (size_t i = 0; i < waypoints.size() - 1; ++i) {
auto& p0 = waypoints[i];
auto& p1 = waypoints[i + 1];
// This just makes the splines look better.
const auto scalar =
1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
vectors.push_back(QuinticControlVector(scalar, p0));
vectors.push_back(QuinticControlVector(scalar, p1));
}
return vectors;
}
void SplineHelper::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) {
auto& f = *solutionVector;
size_t N = d.size();
// Create the temporary vectors
// Note that this is inefficient as it is possible to call
// this function many times. A better implementation would
// pass these temporary matrices by non-const reference to
// save excess allocation and deallocation
std::vector<double> c_star(N, 0.0);
std::vector<double> d_star(N, 0.0);
// This updates the coefficients in the first row
// Note that we should be checking for division by zero here
c_star[0] = c[0] / b[0];
d_star[0] = d[0] / b[0];
// Create the c_star and d_star coefficients in the forward sweep
for (size_t i = 1; i < N; ++i) {
double m = 1.0 / (b[i] - a[i] * c_star[i - 1]);
c_star[i] = c[i] * m;
d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m;
}
f[N - 1] = d_star[N - 1];
// This is the reverse sweep, used to update the solution vector f
for (int i = N - 2; i >= 0; i--) {
f[i] = d_star[i] - c_star[i] * f[i + 1];
}
}

12
wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp
View File

@@ -1,12 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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. */
/*----------------------------------------------------------------------------*/
#include "frc/spline/SplineParameterizer.h"
constexpr units::meter_t frc::SplineParameterizer::kMaxDx;
constexpr units::meter_t frc::SplineParameterizer::kMaxDy;
constexpr units::radian_t frc::SplineParameterizer::kMaxDtheta;

162
wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
View File

@@ -1,162 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/trajectory/Trajectory.h"
#include <units/math.h>
#include <wpi/json.h>
using namespace frc;
bool Trajectory::State::operator==(const Trajectory::State& other) const {
return t == other.t && velocity == other.velocity &&
acceleration == other.acceleration && pose == other.pose &&
curvature == other.curvature;
}
bool Trajectory::State::operator!=(const Trajectory::State& other) const {
return !operator==(other);
}
Trajectory::State Trajectory::State::Interpolate(State endValue,
double i) const {
// Find the new [t] value.
const auto newT = Lerp(t, endValue.t, i);
// Find the delta time between the current state and the interpolated state.
const auto deltaT = newT - t;
// If delta time is negative, flip the order of interpolation.
if (deltaT < 0_s) return endValue.Interpolate(*this, 1.0 - i);
// Check whether the robot is reversing at this stage.
const auto reversing =
velocity < 0_mps ||
(units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq);
// Calculate the new velocity.
// v = v_0 + at
const units::meters_per_second_t newV = velocity + (acceleration * deltaT);
// Calculate the change in position.
// delta_s = v_0 t + 0.5 at^2
const units::meter_t newS =
(velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
(reversing ? -1.0 : 1.0);
// Return the new state. To find the new position for the new state, we need
// to interpolate between the two endpoint poses. The fraction for
// interpolation is the change in position (delta s) divided by the total
// distance between the two endpoints.
const double interpolationFrac =
newS / endValue.pose.Translation().Distance(pose.Translation());
return {newT, newV, acceleration,
Lerp(pose, endValue.pose, interpolationFrac),
Lerp(curvature, endValue.curvature, interpolationFrac)};
}
Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {
m_totalTime = states.back().t;
}
Trajectory::State Trajectory::Sample(units::second_t t) const {
if (t <= m_states.front().t) return m_states.front();
if (t >= m_totalTime) return m_states.back();
// To get the element that we want, we will use a binary search algorithm
// instead of iterating over a for-loop. A binary search is O(std::log(n))
// whereas searching using a loop is O(n).
// This starts at 1 because we use the previous state later on for
// interpolation.
int low = 1;
int high = m_states.size() - 1;
while (low != high) {
int mid = (low + high) / 2;
if (m_states[mid].t < t) {
// This index and everything under it are less than the requested
// timestamp. Therefore, we can discard them.
low = mid + 1;
} else {
// t is at least as large as the element at this index. This means that
// anything after it cannot be what we are looking for.
high = mid;
}
}
// High and Low should be the same.
// The sample's timestamp is now greater than or equal to the requested
// timestamp. If it is greater, we need to interpolate between the
// previous state and the current state to get the exact state that we
// want.
const auto sample = m_states[low];
const auto prevSample = m_states[low - 1];
// If the difference in states is negligible, then we are spot on!
if (units::math::abs(sample.t - prevSample.t) < 1E-9_s) {
return sample;
}
// Interpolate between the two states for the state that we want.
return prevSample.Interpolate(sample,
(t - prevSample.t) / (sample.t - prevSample.t));
}
Trajectory Trajectory::TransformBy(const Transform2d& transform) {
auto& firstState = m_states[0];
auto& firstPose = firstState.pose;
// Calculate the transformed first pose.
auto newFirstPose = firstPose + transform;
auto newStates = m_states;
newStates[0].pose = newFirstPose;
for (unsigned int i = 1; i < newStates.size(); i++) {
auto& state = newStates[i];
// We are transforming relative to the coordinate frame of the new initial
// pose.
state.pose = newFirstPose + (state.pose - firstPose);
}
return Trajectory(newStates);
}
Trajectory Trajectory::RelativeTo(const Pose2d& pose) {
auto newStates = m_states;
for (auto& state : newStates) {
state.pose = state.pose.RelativeTo(pose);
}
return Trajectory(newStates);
}
void frc::to_json(wpi::json& json, const Trajectory::State& state) {
json = wpi::json{{"time", state.t.to<double>()},
{"velocity", state.velocity.to<double>()},
{"acceleration", state.acceleration.to<double>()},
{"pose", state.pose},
{"curvature", state.curvature.to<double>()}};
}
void frc::from_json(const wpi::json& json, Trajectory::State& state) {
state.pose = json.at("pose").get<Pose2d>();
state.t = units::second_t{json.at("time").get<double>()};
state.velocity =
units::meters_per_second_t{json.at("velocity").get<double>()};
state.acceleration =
units::meters_per_second_squared_t{json.at("acceleration").get<double>()};
state.curvature = units::curvature_t{json.at("curvature").get<double>()};
}
bool Trajectory::operator==(const Trajectory& other) const {
return m_states == other.States();
}
bool Trajectory::operator!=(const Trajectory& other) const {
return !operator==(other);
}

118
wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
View File

@@ -1,118 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/trajectory/TrajectoryGenerator.h"
#include <utility>
#include <wpi/raw_ostream.h>
#include "frc/spline/SplineHelper.h"
#include "frc/spline/SplineParameterizer.h"
#include "frc/trajectory/TrajectoryParameterizer.h"
using namespace frc;
const Trajectory TrajectoryGenerator::kDoNothingTrajectory(
std::vector<Trajectory::State>{Trajectory::State()});
std::function<void(const char*)> TrajectoryGenerator::s_errorFunc;
void TrajectoryGenerator::ReportError(const char* error) {
if (s_errorFunc)
s_errorFunc(error);
else
wpi::errs() << "TrajectoryGenerator error: " << error << "\n";
}
Trajectory TrajectoryGenerator::GenerateTrajectory(
Spline<3>::ControlVector initial,
const std::vector<Translation2d>& interiorWaypoints,
Spline<3>::ControlVector end, const TrajectoryConfig& config) {
const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
// Make theta normal for trajectory generation if path is reversed.
// Flip the headings.
if (config.IsReversed()) {
initial.x[1] *= -1;
initial.y[1] *= -1;
end.x[1] *= -1;
end.y[1] *= -1;
}
std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
try {
points =
SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
initial, interiorWaypoints, end));
} catch (SplineParameterizer::MalformedSplineException& e) {
ReportError(e.what());
return kDoNothingTrajectory;
}
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
if (config.IsReversed()) {
for (auto& point : points) {
point = {point.first + flip, -point.second};
}
}
return TrajectoryParameterizer::TimeParameterizeTrajectory(
points, config.Constraints(), config.StartVelocity(),
config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
config.IsReversed());
}
Trajectory TrajectoryGenerator::GenerateTrajectory(
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end, const TrajectoryConfig& config) {
auto [startCV, endCV] = SplineHelper::CubicControlVectorsFromWaypoints(
start, interiorWaypoints, end);
return GenerateTrajectory(startCV, interiorWaypoints, endCV, config);
}
Trajectory TrajectoryGenerator::GenerateTrajectory(
std::vector<Spline<5>::ControlVector> controlVectors,
const TrajectoryConfig& config) {
const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
// Make theta normal for trajectory generation if path is reversed.
if (config.IsReversed()) {
for (auto& vector : controlVectors) {
// Flip the headings.
vector.x[1] *= -1;
vector.y[1] *= -1;
}
}
std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
try {
points = SplinePointsFromSplines(
SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
} catch (SplineParameterizer::MalformedSplineException& e) {
ReportError(e.what());
return kDoNothingTrajectory;
}
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
if (config.IsReversed()) {
for (auto& point : points) {
point = {point.first + flip, -point.second};
}
}
return TrajectoryParameterizer::TimeParameterizeTrajectory(
points, config.Constraints(), config.StartVelocity(),
config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
config.IsReversed());
}
Trajectory TrajectoryGenerator::GenerateTrajectory(
const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
return GenerateTrajectory(
SplineHelper::QuinticControlVectorsFromWaypoints(waypoints), config);
}

237
wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
View File

@@ -1,237 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
/*
* 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.
*/
#include "frc/trajectory/TrajectoryParameterizer.h"
#include <string>
#include <units/math.h>
using namespace frc;
Trajectory TrajectoryParameterizer::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) {
std::vector<ConstrainedState> constrainedStates(points.size());
ConstrainedState predecessor{points.front(), 0_m, startVelocity,
-maxAcceleration, maxAcceleration};
constrainedStates[0] = predecessor;
// Forward pass
for (unsigned int i = 0; i < points.size(); i++) {
auto& constrainedState = constrainedStates[i];
constrainedState.pose = points[i];
// Begin constraining based on predecessor
units::meter_t ds = constrainedState.pose.first.Translation().Distance(
predecessor.pose.first.Translation());
constrainedState.distance = ds + predecessor.distance;
// We may need to iterate to find the maximum end velocity and common
// acceleration, since acceleration limits may be a function of velocity.
while (true) {
// Enforce global max velocity and max reachable velocity by global
// acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
constrainedState.maxVelocity = units::math::min(
maxVelocity,
units::math::sqrt(predecessor.maxVelocity * predecessor.maxVelocity +
predecessor.maxAcceleration * ds * 2.0));
constrainedState.minAcceleration = -maxAcceleration;
constrainedState.maxAcceleration = maxAcceleration;
// At this point, the constrained state is fully constructed apart from
// all the custom-defined user constraints.
for (const auto& constraint : constraints) {
constrainedState.maxVelocity = units::math::min(
constrainedState.maxVelocity,
constraint->MaxVelocity(constrainedState.pose.first,
constrainedState.pose.second,
constrainedState.maxVelocity));
}
// Now enforce all acceleration limits.
EnforceAccelerationLimits(reversed, constraints, &constrainedState);
if (ds.to<double>() < kEpsilon) break;
// If the actual acceleration for this state is higher than the max
// acceleration that we applied, then we need to reduce the max
// acceleration of the predecessor and try again.
units::meters_per_second_squared_t actualAcceleration =
(constrainedState.maxVelocity * constrainedState.maxVelocity -
predecessor.maxVelocity * predecessor.maxVelocity) /
(ds * 2.0);
// If we violate the max acceleration constraint, let's modify the
// predecessor.
if (constrainedState.maxAcceleration < actualAcceleration - 1E-6_mps_sq) {
predecessor.maxAcceleration = constrainedState.maxAcceleration;
} else {
// Constrain the predecessor's max acceleration to the current
// acceleration.
if (actualAcceleration > predecessor.minAcceleration + 1E-6_mps_sq) {
predecessor.maxAcceleration = actualAcceleration;
}
// If the actual acceleration is less than the predecessor's min
// acceleration, it will be repaired in the backward pass.
break;
}
}
predecessor = constrainedState;
}
ConstrainedState successor{points.back(), constrainedStates.back().distance,
endVelocity, -maxAcceleration, maxAcceleration};
// Backward pass
for (int i = points.size() - 1; i >= 0; i--) {
auto& constrainedState = constrainedStates[i];
units::meter_t ds =
constrainedState.distance - successor.distance; // negative
while (true) {
// Enforce max velocity limit (reverse)
// vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
units::meters_per_second_t newMaxVelocity =
units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
successor.minAcceleration * ds * 2.0);
// No more limits to impose! This state can be finalized.
if (newMaxVelocity >= constrainedState.maxVelocity) break;
constrainedState.maxVelocity = newMaxVelocity;
// Check all acceleration constraints with the new max velocity.
EnforceAccelerationLimits(reversed, constraints, &constrainedState);
if (ds.to<double>() > -kEpsilon) break;
// If the actual acceleration for this state is lower than the min
// acceleration, then we need to lower the min acceleration of the
// successor and try again.
units::meters_per_second_squared_t actualAcceleration =
(constrainedState.maxVelocity * constrainedState.maxVelocity -
successor.maxVelocity * successor.maxVelocity) /
(ds * 2.0);
if (constrainedState.minAcceleration > actualAcceleration + 1E-6_mps_sq) {
successor.minAcceleration = constrainedState.minAcceleration;
} else {
successor.minAcceleration = actualAcceleration;
break;
}
}
successor = constrainedState;
}
// Now we can integrate the constrained states forward in time to obtain our
// trajectory states.
std::vector<Trajectory::State> states(points.size());
units::second_t t = 0_s;
units::meter_t s = 0_m;
units::meters_per_second_t v = 0_mps;
for (unsigned int i = 0; i < constrainedStates.size(); i++) {
auto state = constrainedStates[i];
// Calculate the change in position between the current state and the
// previous state.
units::meter_t ds = state.distance - s;
// Calculate the acceleration between the current state and the previous
// state.
units::meters_per_second_squared_t accel =
(state.maxVelocity * state.maxVelocity - v * v) / (ds * 2);
// Calculate dt.
units::second_t dt = 0_s;
if (i > 0) {
states.at(i - 1).acceleration = reversed ? -accel : accel;
if (units::math::abs(accel) > 1E-6_mps_sq) {
// v_f = v_0 + a * t
dt = (state.maxVelocity - v) / accel;
} else if (units::math::abs(v) > 1E-6_mps) {
// delta_x = v * t
dt = ds / v;
} else {
throw std::runtime_error("Something went wrong at iteration " +
std::to_string(i) +
" of time parameterization.");
}
}
v = state.maxVelocity;
s = state.distance;
t += dt;
states[i] = {t, reversed ? -v : v, reversed ? -accel : accel,
state.pose.first, state.pose.second};
}
return Trajectory(states);
}
void TrajectoryParameterizer::EnforceAccelerationLimits(
bool reverse,
const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
ConstrainedState* state) {
for (auto&& constraint : constraints) {
double factor = reverse ? -1.0 : 1.0;
auto minMaxAccel = constraint->MinMaxAcceleration(
state->pose.first, state->pose.second, state->maxVelocity * factor);
if (minMaxAccel.minAcceleration > minMaxAccel.maxAcceleration) {
throw std::runtime_error(
"The constraint's min acceleration was greater than its max "
"acceleration. To debug this, remove all constraints from the config "
"and add each one individually. If the offending constraint was "
"packaged with WPILib, please file a bug report.");
}
state->minAcceleration = units::math::max(
state->minAcceleration,
reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration);
state->maxAcceleration = units::math::min(
state->maxAcceleration,
reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration);
}
}

58
wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
View File

@@ -1,58 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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. */
/*----------------------------------------------------------------------------*/
#include "frc/trajectory/TrajectoryUtil.h"
#include <system_error>
#include <wpi/SmallString.h>
#include <wpi/json.h>
#include <wpi/raw_istream.h>
#include <wpi/raw_ostream.h>
using namespace frc;
void TrajectoryUtil::ToPathweaverJson(const Trajectory& trajectory,
const wpi::Twine& path) {
std::error_code error_code;
wpi::SmallString<128> buf;
wpi::raw_fd_ostream output{path.toStringRef(buf), error_code};
if (error_code) {
throw std::runtime_error(("Cannot open file: " + path).str());
}
wpi::json json = trajectory.States();
output << json;
output.flush();
}
Trajectory TrajectoryUtil::FromPathweaverJson(const wpi::Twine& path) {
std::error_code error_code;
wpi::SmallString<128> buf;
wpi::raw_fd_istream input{path.toStringRef(buf), error_code};
if (error_code) {
throw std::runtime_error(("Cannot open file: " + path).str());
}
wpi::json json;
input >> json;
return Trajectory{json.get<std::vector<Trajectory::State>>()};
}
std::string TrajectoryUtil::SerializeTrajectory(const Trajectory& trajectory) {
wpi::json json = trajectory.States();
return json.dump();
}
Trajectory TrajectoryUtil::DeserializeTrajectory(
const wpi::StringRef json_str) {
wpi::json json = wpi::json::parse(json_str);
return Trajectory{json.get<std::vector<Trajectory::State>>()};
}

42
wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
View File

@@ -1,42 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
#include <units/math.h>
using namespace frc;
CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
units::meters_per_second_squared_t maxCentripetalAcceleration)
: m_maxCentripetalAcceleration(maxCentripetalAcceleration) {}
units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
// ac = v^2 / r
// k (curvature) = 1 / r
// therefore, ac = v^2 * k
// ac / k = v^2
// v = std::sqrt(ac / k)
// We have to multiply by 1_rad here to get the units to cancel out nicely.
// The units library defines a unit for radians although it is technically
// unitless.
return units::math::sqrt(m_maxCentripetalAcceleration /
units::math::abs(curvature) * 1_rad);
}
TrajectoryConstraint::MinMax
CentripetalAccelerationConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
// The acceleration of the robot has no impact on the centripetal acceleration
// of the robot.
return {};
}

31
wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
View File

@@ -1,31 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
using namespace frc;
DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
: m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
wheelSpeeds.Normalize(m_maxSpeed);
return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
}
TrajectoryConstraint::MinMax
DifferentialDriveKinematicsConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
return {};
}

101
wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
View File

@@ -1,101 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
#include <algorithm>
#include <limits>
#include <units/math.h>
#include <wpi/MathExtras.h>
using namespace frc;
DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
SimpleMotorFeedforward<units::meter> feedforward,
DifferentialDriveKinematics kinematics, units::volt_t maxVoltage)
: m_feedforward(feedforward),
m_kinematics(kinematics),
m_maxVoltage(maxVoltage) {}
units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
return units::meters_per_second_t(std::numeric_limits<double>::max());
}
TrajectoryConstraint::MinMax
DifferentialDriveVoltageConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
auto maxWheelSpeed = std::max(wheelSpeeds.left, wheelSpeeds.right);
auto minWheelSpeed = std::min(wheelSpeeds.left, wheelSpeeds.right);
// Calculate maximum/minimum possible accelerations from motor dynamics
// and max/min wheel speeds
auto maxWheelAcceleration =
m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
auto minWheelAcceleration =
m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelSpeed);
// Robot chassis turning on radius = 1/|curvature|. Outer wheel has radius
// increased by half of the trackwidth T. Inner wheel has radius decreased
// by half of the trackwidth. Achassis / radius = Aouter / (radius + T/2), so
// Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 +
// |curvature|T/2). Inner wheel is similar.
// sgn(speed) term added to correctly account for which wheel is on
// outside of turn:
// If moving forward, max acceleration constraint corresponds to wheel on
// outside of turn If moving backward, max acceleration constraint corresponds
// to wheel on inside of turn
// When velocity is zero, then wheel velocities are uniformly zero (robot
// cannot be turning on its center) - we have to treat this as a special case,
// as it breaks the signum function. Both max and min acceleration are
// *reduced in magnitude* in this case.
units::meters_per_second_squared_t maxChassisAcceleration;
units::meters_per_second_squared_t minChassisAcceleration;
if (speed == 0_mps) {
maxChassisAcceleration =
maxWheelAcceleration /
(1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
minChassisAcceleration =
minWheelAcceleration /
(1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
} else {
maxChassisAcceleration =
maxWheelAcceleration /
(1 + m_kinematics.trackWidth * units::math::abs(curvature) *
wpi::sgn(speed) / (2_rad));
minChassisAcceleration =
minWheelAcceleration /
(1 - m_kinematics.trackWidth * units::math::abs(curvature) *
wpi::sgn(speed) / (2_rad));
}
// When turning about a point inside of the wheelbase (i.e. radius less than
// half the trackwidth), the inner wheel's direction changes, but the
// magnitude remains the same. The formula above changes sign for the inner
// wheel when this happens. We can accurately account for this by simply
// negating the inner wheel.
if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) {
if (speed > 0_mps) {
minChassisAcceleration = -minChassisAcceleration;
} else if (speed < 0_mps) {
maxChassisAcceleration = -maxChassisAcceleration;
}
}
return {minChassisAcceleration, maxChassisAcceleration};
}

56
wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp
View File

@@ -1,56 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "frc/trajectory/constraint/EllipticalRegionConstraint.h"
#include <limits>
#include <units/math.h>
using namespace frc;
EllipticalRegionConstraint::EllipticalRegionConstraint(
const Translation2d& center, units::meter_t xWidth, units::meter_t yWidth,
const Rotation2d& rotation, const TrajectoryConstraint& constraint)
: m_center(center),
m_radii(xWidth / 2.0, yWidth / 2.0),
m_constraint(constraint) {
m_radii = m_radii.RotateBy(rotation);
}
units::meters_per_second_t EllipticalRegionConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
if (IsPoseInRegion(pose)) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
return units::meters_per_second_t(std::numeric_limits<double>::infinity());
}
}
TrajectoryConstraint::MinMax EllipticalRegionConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
if (IsPoseInRegion(pose)) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
} else {
return {};
}
}
bool EllipticalRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
// The region (disk) bounded by the ellipse is given by the equation:
// ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
// If the inequality is satisfied, then it is inside the ellipse; otherwise
// it is outside the ellipse.
// Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
return units::math::pow<2>(pose.X() - m_center.X()) *
units::math::pow<2>(m_radii.Y()) +
units::math::pow<2>(pose.Y() - m_center.Y()) *
units::math::pow<2>(m_radii.X()) <=
units::math::pow<2>(m_radii.X()) * units::math::pow<2>(m_radii.Y());
}

37
wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
View File

@@ -1,37 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
#include <units/math.h>
using namespace frc;
MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint(
MecanumDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
: m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
units::meters_per_second_t MecanumDriveKinematicsConstraint::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.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature});
wheelSpeeds.Normalize(m_maxSpeed);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
}
TrajectoryConstraint::MinMax
MecanumDriveKinematicsConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
return {};
}

44
wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp
View File

@@ -1,44 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "frc/trajectory/constraint/RectangularRegionConstraint.h"
#include <limits>
using namespace frc;
RectangularRegionConstraint::RectangularRegionConstraint(
const Translation2d& bottomLeftPoint, const Translation2d& topRightPoint,
const TrajectoryConstraint& constraint)
: m_bottomLeftPoint(bottomLeftPoint),
m_topRightPoint(topRightPoint),
m_constraint(constraint) {}
units::meters_per_second_t RectangularRegionConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
if (IsPoseInRegion(pose)) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
return units::meters_per_second_t(std::numeric_limits<double>::infinity());
}
}
TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const {
if (IsPoseInRegion(pose)) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
} else {
return {};
}
}
bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
return pose.X() >= m_bottomLeftPoint.X() && pose.X() <= m_topRightPoint.X() &&
pose.Y() >= m_bottomLeftPoint.Y() && pose.Y() <= m_topRightPoint.Y();
}