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

Add geometry classes (#1766)

Browse Source 
These classes introduce ways to represent poses and provide easy ways to transform, rotate, and translate poses across 2d space. This classes will be especially useful for a planned odometry and kinematics suite.

Furthermore, these classes can also be used to simply represent waypoints on a field, do superstructure motion planning, etc.
This commit is contained in:
Prateek Machiraju
2019-07-24 02:57:39 -04:00
committed by Peter Johnson
parent 48fe54271a
commit ee24101696
22 changed files with 1898 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

136
wpilibc/src/main/native/include/frc/geometry/Pose2d.h Normal file
View File

@@ -0,0 +1,136 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "Transform2d.h"
#include "Translation2d.h"
#include "Twist2d.h"
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(double x, double 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 underlying translation.
*
* @return Reference to the translational component of the pose.
*/
const Translation2d& Translation() const { return m_translation; }
/**
* 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 .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;
private:
Translation2d m_translation;
Rotation2d m_rotation;
};
} // namespace frc

172
wpilibc/src/main/native/include/frc/geometry/Rotation2d.h Normal file
View File

@@ -0,0 +1,172 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
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.
*/
explicit Rotation2d(double value);
/**
* 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);
/**
* Constructs and returns a Rotation2d with the given degree value.
*
* @param degrees The value of the angle in degrees.
*
* @return The rotation object with the desired angle value.
*/
static Rotation2d FromDegrees(double degrees);
/**
* Adds two rotations together, with the result being bounded between -kPi and
* kPi.
*
* For example, Rotation2d.FromDegrees(30) + Rotation2d.FromDegrees(60) =
* Rotation2d{-kPi/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{-kPi/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;
/**
* 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.
*/
double Radians() const { return m_value; }
/**
* Returns the degree value of the rotation.
*
* @return The degree value of the rotation.
*/
double Degrees() const { return Rad2Deg(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:
double m_value = 0;
double m_cos = 1;
double m_sin = 0;
constexpr static double kPi = 3.14159265358979323846;
template <typename T>
static T Rad2Deg(const T& rad) {
return rad * 180.0 / kPi;
}
template <typename T>
static T Deg2Rad(const T& deg) {
return deg * kPi / 180.0;
}
};
} // namespace frc

60
wpilibc/src/main/native/include/frc/geometry/Transform2d.h Normal file
View File

@@ -0,0 +1,60 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#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 rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
const Rotation2d& Rotation() const { return m_rotation; }
private:
Translation2d m_translation;
Rotation2d m_rotation;
};
} // namespace frc

196
wpilibc/src/main/native/include/frc/geometry/Translation2d.h Normal file
View File

@@ -0,0 +1,196 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "Rotation2d.h"
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(double x, double 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.
*/
double Distance(const Translation2d& other) const;
/**
* Returns the X component of the translation.
*
* @return The x component of the translation.
*/
double X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
* @return The y component of the translation.
*/
double Y() const { return m_y; }
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
double 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;
/*
* 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:
double m_x = 0;
double m_y = 0;
};
} // namespace frc

34
wpilibc/src/main/native/include/frc/geometry/Twist2d.h Normal file
View File

@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
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
*/
double dx = 0;
/**
* Linear "dy" component
*/
double dy = 0;
/**
* Angular "dtheta" component (radians)
*/
double dtheta = 0;
};
} // namespace frc