Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

34
wpimath/src/main/native/cpp/MathShared.cpp Normal file
View File

@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* 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 "math/MathShared.h"
#include <wpi/mutex.h>
using namespace wpi::math;
namespace {
class DefaultMathShared : public MathShared {
public:
void ReportError(const wpi::Twine& error) override {}
void ReportUsage(MathUsageId id, int count) override {}
};
} // namespace
static std::unique_ptr<MathShared> mathShared;
static wpi::mutex setLock;
MathShared& MathSharedStore::GetMathShared() {
std::scoped_lock lock(setLock);
if (!mathShared) mathShared = std::make_unique<DefaultMathShared>();
return *mathShared;
}
void MathSharedStore::SetMathShared(std::unique_ptr<MathShared> shared) {
std::scoped_lock lock(setLock);
mathShared = std::move(shared);
}

87
wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp Normal file
View File

@@ -0,0 +1,87 @@
// This file contains the implementation of both drake_assert and drake_throw.
/* clang-format off to disable clang-format-includes */
#include "drake/common/drake_assert.h"
#include "drake/common/drake_throw.h"
/* clang-format on */
#include <atomic>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include "drake/common/drake_assertion_error.h"
#include "drake/common/never_destroyed.h"
namespace drake {
namespace internal {
namespace {
// Singleton to manage assertion configuration.
struct AssertionConfig {
static AssertionConfig& singleton() {
static never_destroyed<AssertionConfig> global;
return global.access();
}
std::atomic<bool> assertion_failures_are_exceptions;
};
// Stream into @p out the given failure details; only @p condition may be null.
void PrintFailureDetailTo(std::ostream& out, const char* condition,
const char* func, const char* file, int line) {
out << "Failure at " << file << ":" << line << " in " << func << "()";
if (condition) {
out << ": condition '" << condition << "' failed.";
} else {
out << ".";
}
}
} // namespace
// Declared in drake_assert.h.
void Abort(const char* condition, const char* func, const char* file,
int line) {
std::cerr << "abort: ";
PrintFailureDetailTo(std::cerr, condition, func, file, line);
std::cerr << std::endl;
std::abort();
}
// Declared in drake_throw.h.
void Throw(const char* condition, const char* func, const char* file,
int line) {
std::ostringstream what;
PrintFailureDetailTo(what, condition, func, file, line);
throw assertion_error(what.str().c_str());
}
// Declared in drake_assert.h.
void AssertionFailed(const char* condition, const char* func, const char* file,
int line) {
if (AssertionConfig::singleton().assertion_failures_are_exceptions) {
Throw(condition, func, file, line);
} else {
Abort(condition, func, file, line);
}
}
} // namespace internal
} // namespace drake
// Configures the DRAKE_ASSERT and DRAKE_DEMAND assertion failure handling
// behavior.
//
// By default, assertion failures will result in an ::abort(). If this method
// has ever been called, failures will result in a thrown exception instead.
//
// Assertion configuration has process-wide scope. Changes here will affect
// all assertions within the current process.
//
// This method is intended ONLY for use by pydrake bindings, and thus is not
// declared in any header file, to discourage anyone from using it.
extern "C" void drake_set_assertion_failure_to_throw_exception() {
drake::internal::AssertionConfig::singleton().
assertion_failures_are_exceptions = true;
}

463
wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp Normal file
View File

@@ -0,0 +1,463 @@
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_throw.h"
#include "drake/common/is_approx_equal_abstol.h"
#include <Eigen/Eigenvalues>
#include <Eigen/QR>
// This code has https://github.com/RobotLocomotion/drake/pull/11118 applied to
// fix an infinite loop in reorder_eigen().
namespace drake {
namespace math {
namespace {
/* helper functions */
template <typename T>
int sgn(T val) {
return (T(0) < val) - (val < T(0));
}
void check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B) {
// This function checks if (A,B) is a stabilizable pair.
// (A,B) is stabilizable if and only if the uncontrollable eigenvalues of
// A, if any, have absolute values less than one, where an eigenvalue is
// uncontrollable if Rank[lambda * I - A, B] < n.
int n = B.rows(), m = B.cols();
Eigen::EigenSolver<Eigen::MatrixXd> es(A);
for (int i = 0; i < n; i++) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
1)
continue;
Eigen::MatrixXcd E(n, n + m);
E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
DRAKE_THROW_UNLESS(qr.rank() == n);
}
}
void check_detectable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& Q) {
// This function check if (A,C) is a detectable pair, where Q = C' * C.
// (A,C) is detectable if and only if the unobservable eigenvalues of A,
// if any, have absolute values less than one, where an eigenvalue is
// unobservable if Rank[lambda * I - A; C] < n.
// Also, (A,C) is detectable if and only if (A',C') is stabilizable.
int n = A.rows();
Eigen::LDLT<Eigen::MatrixXd> ldlt(Q);
Eigen::MatrixXd L = ldlt.matrixL();
Eigen::MatrixXd D = ldlt.vectorD();
Eigen::MatrixXd D_sqrt = Eigen::MatrixXd::Zero(n, n);
for (int i = 0; i < n; i++) {
D_sqrt(i, i) = sqrt(D(i));
}
Eigen::MatrixXd C = L * D_sqrt;
check_stabilizable(A.transpose(), C.transpose());
}
// "Givens rotation" computes an orthogonal 2x2 matrix R such that
// it eliminates the 2nd coordinate of the vector [a,b]', i.e.,
// R * [ a ] = [ a_hat ]
// [ b ] [ 0 ]
// The implementation is based on
// https://en.wikipedia.org/wiki/Givens_rotation#Stable_calculation
void Givens_rotation(double a, double b, Eigen::Ref<Eigen::Matrix2d> R,
double eps = 1e-10) {
double c, s;
if (fabs(b) < eps) {
c = (a < -eps ? -1 : 1);
s = 0;
} else if (fabs(a) < eps) {
c = 0;
s = -sgn(b);
} else if (fabs(a) > fabs(b)) {
double t = b / a;
double u = sgn(a) * fabs(sqrt(1 + t * t));
c = 1 / u;
s = -c * t;
} else {
double t = a / b;
double u = sgn(b) * fabs(sqrt(1 + t * t));
s = -1 / u;
c = -s * t;
}
R(0, 0) = c, R(0, 1) = -s, R(1, 0) = s, R(1, 1) = c;
}
// The arguments S, T, and Z will be changed.
void swap_block_11(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> Z, int p) {
// Dooren, Case I, p124-125.
int n2 = S.rows();
Eigen::Matrix2d A = S.block<2, 2>(p, p);
Eigen::Matrix2d B = T.block<2, 2>(p, p);
Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
Eigen::Matrix2d H = A(1, 1) * B - B(1, 1) * A;
Givens_rotation(H(0, 1), H(0, 0), Z1.block<2, 2>(p, p));
S = (S * Z1).eval();
T = (T * Z1).eval();
Z = (Z * Z1).eval();
Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(n2, n2);
Givens_rotation(T(p, p), T(p + 1, p), Q.block<2, 2>(p, p));
S = (Q * S).eval();
T = (Q * T).eval();
S(p + 1, p) = 0;
T(p + 1, p) = 0;
}
// The arguments S, T, and Z will be changed.
void swap_block_21(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> Z, int p) {
// Dooren, Case II, p126-127.
int n2 = S.rows();
Eigen::Matrix3d A = S.block<3, 3>(p, p);
Eigen::Matrix3d B = T.block<3, 3>(p, p);
// Compute H and eliminate H(1,0) by row operation.
Eigen::Matrix3d H = A(2, 2) * B - B(2, 2) * A;
Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3);
Givens_rotation(H(0, 0), H(1, 0), R.block<2, 2>(0, 0));
H = (R * H).eval();
Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2);
Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
// Compute Z1, Z2, Q1, Q2.
Givens_rotation(H(1, 2), H(1, 1), Z1.block<2, 2>(p + 1, p + 1));
H = (H * Z1.block<3, 3>(p, p)).eval();
Givens_rotation(H(0, 1), H(0, 0), Z2.block<2, 2>(p, p));
S = (S * Z1).eval();
T = (T * Z1).eval();
Z = (Z * Z1 * Z2).eval();
Givens_rotation(T(p + 1, p + 1), T(p + 2, p + 1),
Q1.block<2, 2>(p + 1, p + 1));
S = (Q1 * S * Z2).eval();
T = (Q1 * T * Z2).eval();
Givens_rotation(T(p, p), T(p + 1, p), Q2.block<2, 2>(p, p));
S = (Q2 * S).eval();
T = (Q2 * T).eval();
S(p + 1, p) = 0;
S(p + 2, p) = 0;
T(p + 1, p) = 0;
T(p + 2, p) = 0;
T(p + 2, p + 1) = 0;
}
// The arguments S, T, and Z will be changed.
void swap_block_12(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> Z, int p) {
int n2 = S.rows();
// Swap the role of S and T.
Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2);
Eigen::MatrixXd Q0 = Eigen::MatrixXd::Identity(n2, n2);
Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2);
Givens_rotation(S(p + 1, p + 1), S(p + 2, p + 1),
Q0.block<2, 2>(p + 1, p + 1));
S = (Q0 * S).eval();
T = (Q0 * T).eval();
Eigen::Matrix3d A = S.block<3, 3>(p, p);
Eigen::Matrix3d B = T.block<3, 3>(p, p);
// Compute H and eliminate H(2,1) by column operation.
Eigen::Matrix3d H = B(0, 0) * A - A(0, 0) * B;
Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3);
Givens_rotation(H(2, 2), H(2, 1), R.block<2, 2>(1, 1));
H = (H * R).eval();
// Compute Q1, Q2, Z1, Z2.
Givens_rotation(H(0, 1), H(1, 1), Q1.block<2, 2>(p, p));
H = (Q1.block<3, 3>(p, p) * H).eval();
Givens_rotation(H(1, 2), H(2, 2), Q2.block<2, 2>(p + 1, p + 1));
S = (Q1 * S).eval();
T = (Q1 * T).eval();
Givens_rotation(S(p + 1, p + 1), S(p + 1, p), Z1.block<2, 2>(p, p));
S = (Q2 * S * Z1).eval();
T = (Q2 * T * Z1).eval();
Givens_rotation(S(p + 2, p + 2), S(p + 2, p + 1),
Z2.block<2, 2>(p + 1, p + 1));
S = (S * Z2).eval();
T = (T * Z2).eval();
Z = (Z * Z1 * Z2).eval();
// Swap back the role of S and T.
Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p));
S = (Q3 * S).eval();
T = (Q3 * T).eval();
S(p + 2, p) = 0;
S(p + 2, p + 1) = 0;
T(p + 1, p) = 0;
T(p + 2, p) = 0;
T(p + 2, p + 1) = 0;
}
// The arguments S, T, and Z will be changed.
void swap_block_22(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> Z, int p) {
// Direct Swapping Algorithm based on
// "Numerical Methods for General and Structured Eigenvalue Problems" by
// Daniel Kressner, p108-111.
// ( http://sma.epfl.ch/~anchpcommon/publications/kressner_eigenvalues.pdf ).
// Also relevant but not applicable here:
// "On Swapping Diagonal Blocks in Real Schur Form" by Zhaojun Bai and James
// W. Demmelt;
int n2 = S.rows();
Eigen::MatrixXd A = S.block<4, 4>(p, p);
Eigen::MatrixXd B = T.block<4, 4>(p, p);
// Solve
// A11 * X - Y A22 = A12
// B11 * X - Y B22 = B12
// Reduce to solve Cx=D, where x=[x1;...;x4;y1;...;y4].
Eigen::Matrix<double, 8, 8> C = Eigen::Matrix<double, 8, 8>::Zero();
Eigen::Matrix<double, 8, 1> D;
// clang-format off
C << A(0, 0), 0, A(0, 1), 0, -A(2, 2), -A(3, 2), 0, 0,
0, A(0, 0), 0, A(0, 1), -A(2, 3), -A(3, 3), 0, 0,
A(1, 0), 0, A(1, 1), 0, 0, 0, -A(2, 2), -A(3, 2),
0, A(1, 0), 0, A(1, 1), 0, 0, -A(2, 3), -A(3, 3),
B(0, 0), 0, B(0, 1), 0, -B(2, 2), -B(3, 2), 0, 0,
0, B(0, 0), 0, B(0, 1), -B(2, 3), -B(3, 3), 0, 0,
B(1, 0), 0, B(1, 1), 0, 0, 0, -B(2, 2), -B(3, 2),
0, B(1, 0), 0, B(1, 1), 0, 0, -B(2, 3), -B(3, 3);
// clang-format on
D << A(0, 2), A(0, 3), A(1, 2), A(1, 3), B(0, 2), B(0, 3), B(1, 2), B(1, 3);
Eigen::MatrixXd x = C.colPivHouseholderQr().solve(D);
// Q * [ -Y ] = [ R_Y ] , Z' * [ -X ] = [ R_X ] .
// [ I ] [ 0 ] [ I ] = [ 0 ]
Eigen::Matrix<double, 4, 2> X, Y;
X << -x(0, 0), -x(1, 0), -x(2, 0), -x(3, 0), Eigen::Matrix2d::Identity();
Y << -x(4, 0), -x(5, 0), -x(6, 0), -x(7, 0), Eigen::Matrix2d::Identity();
Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > qr1(X);
Z1.block<4, 4>(p, p) = qr1.householderQ();
Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > qr2(Y);
Q1.block<4, 4>(p, p) = qr2.householderQ().adjoint();
// Apply transform Q1 * (S,T) * Z1.
S = (Q1 * S * Z1).eval();
T = (Q1 * T * Z1).eval();
Z = (Z * Z1).eval();
// Eliminate the T(p+3,p+2) entry.
Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
Givens_rotation(T(p + 2, p + 2), T(p + 3, p + 2),
Q2.block<2, 2>(p + 2, p + 2));
S = (Q2 * S).eval();
T = (Q2 * T).eval();
// Eliminate the T(p+1,p) entry.
Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2);
Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p));
S = (Q3 * S).eval();
T = (Q3 * T).eval();
S(p + 2, p) = 0;
S(p + 2, p + 1) = 0;
S(p + 3, p) = 0;
S(p + 3, p + 1) = 0;
T(p + 1, p) = 0;
T(p + 2, p) = 0;
T(p + 2, p + 1) = 0;
T(p + 3, p) = 0;
T(p + 3, p + 1) = 0;
T(p + 3, p + 2) = 0;
}
// Functionality of "swap_block" function:
// swap the 1x1 or 2x2 blocks pointed by p and q.
// There are four cases: swapping 1x1 and 1x1 matrices, swapping 2x2 and 1x1
// matrices, swapping 1x1 and 2x2 matrices, and swapping 2x2 and 2x2 matrices.
// Algorithms are described in the papers
// "A generalized eigenvalue approach for solving Riccati equations" by P. Van
// Dooren, 1981 ( http://epubs.siam.org/doi/pdf/10.1137/0902010 ), and
// "Numerical Methods for General and Structured Eigenvalue Problems" by
// Daniel Kressner, 2005.
void swap_block(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> Z, int p, int q, int q_block_size,
double eps = 1e-10) {
int p_tmp = q, p_block_size;
while (p_tmp-- > p) {
p_block_size = 1;
if (p_tmp >= 1 && fabs(S(p_tmp, p_tmp - 1)) > eps) {
p_block_size = 2;
p_tmp--;
}
switch (p_block_size * 10 + q_block_size) {
case 11:
swap_block_11(S, T, Z, p_tmp);
break;
case 21:
swap_block_21(S, T, Z, p_tmp);
break;
case 12:
swap_block_12(S, T, Z, p_tmp);
break;
case 22:
swap_block_22(S, T, Z, p_tmp);
break;
}
}
}
// Functionality of "reorder_eigen" function:
// Reorder the eigenvalues of (S,T) such that the top-left n by n matrix has
// stable eigenvalues by multiplying Q's and Z's on the left and the right,
// respectively.
// Stable eigenvalues are inside the unit disk.
//
// Algorithm:
// Go along the diagonals of (S,T) from the top left to the bottom right.
// Once find a stable eigenvalue, push it to top left.
// In implementation, use two pointers, p and q.
// p points to the current block (1x1 or 2x2) and q points to the block with the
// stable eigenvalue(s).
// Push the block pointed by q to the position pointed by p.
// Finish when n stable eigenvalues are placed at the top-left n by n matrix.
// The algorithm for swapping blocks is described in the papers
// "A generalized eigenvalue approach for solving Riccati equations" by P. Van
// Dooren, 1981, and "Numerical Methods for General and Structured Eigenvalue
// Problems" by Daniel Kressner, 2005.
void reorder_eigen(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> Z, double eps = 1e-10) {
// abs(a) < eps => a = 0
int n2 = S.rows();
int n = n2 / 2, p = 0, q = 0;
// Find the first unstable p block.
while (p < n) {
if (fabs(S(p + 1, p)) < eps) { // p block size = 1
if (fabs(T(p, p)) > eps && fabs(S(p, p)) <= fabs(T(p, p))) { // stable
p++;
continue;
}
} else { // p block size = 2
const double det_T =
T(p, p) * T(p + 1, p + 1) - T(p + 1, p) * T(p, p + 1);
if (fabs(det_T) > eps) {
const double det_S =
S(p, p) * S(p + 1, p + 1) - S(p + 1, p) * S(p, p + 1);
if (fabs(det_S) <= fabs(det_T)) { // stable
p += 2;
continue;
}
}
}
break;
}
q = p;
// Make the first n generalized eigenvalues stable.
while (p < n && q < n2) {
// Update q.
int q_block_size = 0;
while (q < n2) {
if (q == n2 - 1 || fabs(S(q + 1, q)) < eps) { // block size = 1
if (fabs(T(q, q)) > eps && fabs(S(q, q)) <= fabs(T(q, q))) {
q_block_size = 1;
break;
}
q++;
} else { // block size = 2
const double det_T =
T(q, q) * T(q + 1, q + 1) - T(q + 1, q) * T(q, q + 1);
if (fabs(det_T) > eps) {
const double det_S =
S(q, q) * S(q + 1, q + 1) - S(q + 1, q) * S(q, q + 1);
if (fabs(det_S) <= fabs(det_T)) {
q_block_size = 2;
break;
}
}
q += 2;
}
}
if (q >= n2)
throw std::runtime_error("fail to find enough stable eigenvalues");
// Swap blocks pointed by p and q.
if (p != q) {
swap_block(S, T, Z, p, q, q_block_size);
p += q_block_size;
q += q_block_size;
}
}
if (p < n && q >= n2)
throw std::runtime_error("fail to find enough stable eigenvalues");
}
} // namespace
/**
* DiscreteAlgebraicRiccatiEquation function
* computes the unique stabilizing solution X to the discrete-time algebraic
* Riccati equation:
* \f[
* A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0
* \f]
*
* @throws std::runtime_error if Q is not positive semi-definite.
* @throws std::runtime_error if R is not positive definite.
*
* Based on the Schur Vector approach outlined in this paper:
* "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
* by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell, in TAC, 1980,
* http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1102434
*
* Note: When, for example, n = 100, m = 80, and entries of A, B, Q_half,
* R_half are sampled from standard normal distributions, where
* Q = Q_half'*Q_half and similar for R, the absolute error of the solution
* is 10^{-6}, while the absolute error of the solution computed by Matlab is
* 10^{-8}.
*
* TODO(weiqiao.han): I may overwrite the RealQZ function to improve the
* accuracy, together with more thorough tests.
*/
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R) {
int n = B.rows(), m = B.cols();
DRAKE_DEMAND(m <= n);
DRAKE_DEMAND(A.rows() == n && A.cols() == n);
DRAKE_DEMAND(Q.rows() == n && Q.cols() == n);
DRAKE_DEMAND(R.rows() == m && R.cols() == m);
DRAKE_DEMAND(is_approx_equal_abstol(Q, Q.transpose(), 1e-10));
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(Q);
for (int i = 0; i < n; i++) {
DRAKE_THROW_UNLESS(es.eigenvalues()[i] >= 0);
}
DRAKE_DEMAND(is_approx_equal_abstol(R, R.transpose(), 1e-10));
Eigen::LLT<Eigen::MatrixXd> R_cholesky(R);
DRAKE_THROW_UNLESS(R_cholesky.info() == Eigen::Success);
check_stabilizable(A, B);
check_detectable(A, Q);
Eigen::MatrixXd M(2 * n, 2 * n), L(2 * n, 2 * n);
M << A, Eigen::MatrixXd::Zero(n, n), -Q, Eigen::MatrixXd::Identity(n, n);
L << Eigen::MatrixXd::Identity(n, n), B * R.inverse() * B.transpose(),
Eigen::MatrixXd::Zero(n, n), A.transpose();
// QZ decomposition of M and L
// QMZ = S, QLZ = T
// where Q and Z are real orthogonal matrixes
// T is upper-triangular matrix, and S is upper quasi-triangular matrix
Eigen::RealQZ<Eigen::MatrixXd> qz(2 * n);
qz.compute(M, L); // M = Q S Z, L = Q T Z (Q and Z computed by Eigen package
// are adjoints of Q and Z above)
Eigen::MatrixXd S = qz.matrixS(), T = qz.matrixT(),
Z = qz.matrixZ().adjoint();
// Reorder the generalized eigenvalues of (S,T).
Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(2 * n, 2 * n);
reorder_eigen(S, T, Z2);
Z = (Z * Z2).eval();
// The first n columns of Z is ( U1 ) .
// ( U2 )
// -1
// X = U2 * U1 is a solution of the discrete time Riccati equation.
Eigen::MatrixXd U1 = Z.block(0, 0, n, n), U2 = Z.block(n, 0, n, n);
Eigen::MatrixXd X = U2 * U1.inverse();
X = (X + X.adjoint().eval()) / 2.0;
return X;
}
} // namespace math
} // namespace drake

110
wpimath/src/main/native/cpp/geometry/Pose2d.cpp Normal file
View File

@@ -0,0 +1,110 @@
/*----------------------------------------------------------------------------*/
/* 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/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>()};
}

87
wpimath/src/main/native/cpp/geometry/Rotation2d.cpp 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. */
/*----------------------------------------------------------------------------*/
#include "frc/geometry/Rotation2d.h"
#include <cmath>
#include <wpi/json.h>
#include "units/math.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
wpimath/src/main/native/cpp/geometry/Transform2d.cpp Normal file
View File

@@ -0,0 +1,40 @@
/*----------------------------------------------------------------------------*/
/* 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);
}

89
wpimath/src/main/native/cpp/geometry/Translation2d.cpp Normal file
View File

@@ -0,0 +1,89 @@
/*----------------------------------------------------------------------------*/
/* 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 <wpi/json.h>
#include "units/math.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>()}};
}

60
wpimath/src/main/native/cpp/jni/DrakeJNI.cpp Normal file
View File

@@ -0,0 +1,60 @@
/*----------------------------------------------------------------------------*/
/* 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 <jni.h>
#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions>
#include <wpi/jni_util.h>
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "edu_wpi_first_wpiutil_math_DrakeJNI.h"
using namespace wpi::java;
extern "C" {
/*
* Class: edu_wpi_first_wpiutil_math_DrakeJNI
* Method: discreteAlgebraicRiccatiEquation
* Signature: ([D[D[D[DII[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_wpiutil_math_DrakeJNI_discreteAlgebraicRiccatiEquation
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jint states, jint inputs, jdoubleArray S)
{
jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Amat{nativeA, states, states};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Bmat{nativeB, states, inputs};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Qmat{nativeQ, states, states};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Rmat{nativeR, inputs, inputs};
Eigen::MatrixXd result =
drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);
env->ReleaseDoubleArrayElements(A, nativeA, 0);
env->ReleaseDoubleArrayElements(B, nativeB, 0);
env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
env->ReleaseDoubleArrayElements(R, nativeR, 0);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
}
} // extern "C"

42
wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp Normal file
View File

@@ -0,0 +1,42 @@
/*----------------------------------------------------------------------------*/
/* 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/DifferentialDriveOdometry.h"
#include "math/MathShared.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;
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1);
}
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
wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp Normal file
View File

@@ -0,0 +1,23 @@
/*----------------------------------------------------------------------------*/
/* 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
wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp Normal file
View File

@@ -0,0 +1,69 @@
/*----------------------------------------------------------------------------*/
/* 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
wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp Normal file
View File

@@ -0,0 +1,43 @@
/*----------------------------------------------------------------------------*/
/* 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/MecanumDriveOdometry.h"
#include "math/MathShared.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;
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
}
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
wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp Normal file
View File

@@ -0,0 +1,36 @@
/*----------------------------------------------------------------------------*/
/* 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
wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp 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. */
/*----------------------------------------------------------------------------*/
#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
wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp Normal file
View File

@@ -0,0 +1,45 @@
/*----------------------------------------------------------------------------*/
/* 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/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
wpimath/src/main/native/cpp/spline/SplineHelper.cpp Normal file
View File

@@ -0,0 +1,214 @@
/*----------------------------------------------------------------------------*/
/* 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
wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp Normal file
View File

@@ -0,0 +1,12 @@
/*----------------------------------------------------------------------------*/
/* 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/SplineParameterizer.h"
constexpr units::meter_t frc::SplineParameterizer::kMaxDx;
constexpr units::meter_t frc::SplineParameterizer::kMaxDy;
constexpr units::radian_t frc::SplineParameterizer::kMaxDtheta;

163
wpimath/src/main/native/cpp/trajectory/Trajectory.cpp Normal file
View File

@@ -0,0 +1,163 @@
/*----------------------------------------------------------------------------*/
/* 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 <wpi/json.h>
#include "units/math.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
wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp Normal file
View File

@@ -0,0 +1,118 @@
/*----------------------------------------------------------------------------*/
/* 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
wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp Normal file
View File

@@ -0,0 +1,237 @@
/*----------------------------------------------------------------------------*/
/* 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
wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp 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. */
/*----------------------------------------------------------------------------*/
#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
wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp Normal file
View File

@@ -0,0 +1,42 @@
/*----------------------------------------------------------------------------*/
/* 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
wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp Normal file
View File

@@ -0,0 +1,31 @@
/*----------------------------------------------------------------------------*/
/* 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 {};
}

102
wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp Normal file
View File

@@ -0,0 +1,102 @@
/*----------------------------------------------------------------------------*/
/* 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 <wpi/MathExtras.h>
#include "units/math.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
wpimath/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp 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. */
/*----------------------------------------------------------------------------*/
#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
wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp 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. */
/*----------------------------------------------------------------------------*/
#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
wpimath/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp 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. */
/*----------------------------------------------------------------------------*/
#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();
}