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

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

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

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

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

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

@@ -0,0 +1,107 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "Translation2d.h"
namespace frc {
class Pose2d;
/**
* Represents a transformation for a Pose2d.
*/
class Transform2d {
public:
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param final The final pose for the transformation.
*/
Transform2d(Pose2d initial, Pose2d final);
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
Transform2d(Translation2d translation, Rotation2d rotation);
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
constexpr Transform2d() = default;
/**
* Returns the translation component of the transformation.
*
* @return Reference to the translational component of the transform.
*/
const Translation2d& Translation() const { return m_translation; }
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
const Rotation2d& Rotation() const { return m_rotation; }
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
Transform2d Inverse() const;
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform2d.
*/
Transform2d operator*(double scalar) const {
return Transform2d(m_translation * scalar, m_rotation * scalar);
}
/**
* Checks equality between this Transform2d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Transform2d& other) const;
/**
* Checks inequality between this Transform2d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Transform2d& other) const;
private:
Translation2d m_translation;
Rotation2d m_rotation;
};
} // namespace frc

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

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

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

@@ -0,0 +1,58 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/angle.h>
#include <units/length.h>
#include <units/math.h>
namespace frc {
/**
* A change in distance along arc since the last pose update. We can use ideas
* from differential calculus to create new Pose2ds from a Twist2d and vise
* versa.
*
* A Twist can be used to represent a difference between two poses.
*/
struct Twist2d {
/**
* Linear "dx" component
*/
units::meter_t dx = 0_m;
/**
* Linear "dy" component
*/
units::meter_t dy = 0_m;
/**
* Angular "dtheta" component (radians)
*/
units::radian_t dtheta = 0_rad;
/**
* Checks equality between this Twist2d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Twist2d& other) const {
return units::math::abs(dx - other.dx) < 1E-9_m &&
units::math::abs(dy - other.dy) < 1E-9_m &&
units::math::abs(dtheta - other.dtheta) < 1E-9_rad;
}
/**
* Checks inequality between this Twist2d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Twist2d& other) const { return !operator==(other); }
};
} // namespace frc