mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
Add geometry classes (#1766)
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
committed by
Peter Johnson
Peter Johnson
parent
48fe54271a
commit
ee24101696
157
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
Normal file
157
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
Normal file
@@ -0,0 +1,157 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.geometry;
|
||||
|
||||
/**
|
||||
* Represents a 2d pose containing translational and rotational elements.
|
||||
*/
|
||||
public class Pose2d {
|
||||
private final Translation2d m_translation;
|
||||
private final Rotation2d m_rotation;
|
||||
|
||||
/**
|
||||
* Constructs a pose at the origin facing toward the positive X axis.
|
||||
* (Translation2d{0, 0} and Rotation{0})
|
||||
*/
|
||||
public Pose2d() {
|
||||
m_translation = new Translation2d();
|
||||
m_rotation = new Rotation2d();
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public Pose2d(Translation2d translation, Rotation2d rotation) {
|
||||
m_translation = translation;
|
||||
m_rotation = 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.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public Pose2d(double x, double y, Rotation2d rotation) {
|
||||
m_translation = new Translation2d(x, y);
|
||||
m_rotation = rotation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Transforms the pose by the given transformation and returns the new
|
||||
* transformed pose.
|
||||
*
|
||||
* <p>The matrix multiplication is as follows
|
||||
* [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.
|
||||
*/
|
||||
public Pose2d plus(Transform2d other) {
|
||||
return transformBy(other);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the translation component of the transformation.
|
||||
*
|
||||
* @return The translational component of the pose.
|
||||
*/
|
||||
public Translation2d getTranslation() {
|
||||
return m_translation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the rotational component of the transformation.
|
||||
*
|
||||
* @return The rotational component of the pose.
|
||||
*/
|
||||
public Rotation2d getRotation() {
|
||||
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.
|
||||
*/
|
||||
public Pose2d transformBy(Transform2d other) {
|
||||
return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
|
||||
m_rotation.plus(other.getRotation()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the other pose relative to the current pose.
|
||||
*
|
||||
* <p>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.
|
||||
*/
|
||||
public Pose2d relativeTo(Pose2d other) {
|
||||
var transform = new Transform2d(other, this);
|
||||
return new Pose2d(transform.getTranslation(), transform.getRotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Obtain a new Pose2d from a (constant curvature) velocity.
|
||||
*
|
||||
* <p>See <a href="https://file.tavsys.net/control/state-space-guide.pdf">
|
||||
* Controls Engineering in the FIRST Robotics Competition</a>
|
||||
* section on nonlinear pose estimation for derivation.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>"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.
|
||||
*/
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public Pose2d exp(Twist2d twist) {
|
||||
double dx = twist.dx;
|
||||
double dy = twist.dy;
|
||||
double dtheta = twist.dtheta;
|
||||
|
||||
double sinTheta = Math.sin(dtheta);
|
||||
double cosTheta = Math.cos(dtheta);
|
||||
|
||||
double s;
|
||||
double c;
|
||||
if (Math.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;
|
||||
}
|
||||
var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
|
||||
new Rotation2d(cosTheta, sinTheta));
|
||||
|
||||
return this.plus(transform);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,170 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.geometry;
|
||||
|
||||
/**
|
||||
* A rotation in a 2d coordinate frame represented a point on the unit circle
|
||||
* (cosine and sine).
|
||||
*/
|
||||
public class Rotation2d {
|
||||
private final double m_value;
|
||||
private final double m_cos;
|
||||
private final double m_sin;
|
||||
|
||||
/**
|
||||
* Constructs a Rotation2d with a default angle of 0 degrees.
|
||||
*/
|
||||
public Rotation2d() {
|
||||
m_value = 0.0;
|
||||
m_cos = 1.0;
|
||||
m_sin = 0.0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a Rotation2d with the given radian value.
|
||||
* The x and y don't have to be normalized.
|
||||
*
|
||||
* @param value The value of the angle in radians.
|
||||
*/
|
||||
public Rotation2d(double value) {
|
||||
m_value = value;
|
||||
m_cos = Math.cos(value);
|
||||
m_sin = Math.sin(value);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a Rotation2d with the given x and y (cosine and sine)
|
||||
* components.
|
||||
*
|
||||
* @param x The x component or cosine of the rotation.
|
||||
* @param y The y component or sine of the rotation.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public Rotation2d(double x, double y) {
|
||||
double magnitude = Math.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 = Math.atan2(m_sin, m_cos);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public static Rotation2d fromDegrees(double degrees) {
|
||||
return new Rotation2d(Math.toRadians(degrees));
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds two rotations together, with the result being bounded between -kPi and
|
||||
* kPi.
|
||||
*
|
||||
* <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) =
|
||||
* Rotation2d{-kPi/2}
|
||||
*
|
||||
* @param other The rotation to add.
|
||||
* @return The sum of the two rotations.
|
||||
*/
|
||||
public Rotation2d plus(Rotation2d other) {
|
||||
return rotateBy(other);
|
||||
}
|
||||
|
||||
/**
|
||||
* Subtracts the new rotation from the current rotation and returns the new
|
||||
* rotation.
|
||||
*
|
||||
* <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) =
|
||||
* Rotation2d{-kPi/2}
|
||||
*
|
||||
* @param other The rotation to subtract.
|
||||
* @return The difference between the two rotations.
|
||||
*/
|
||||
public Rotation2d minus(Rotation2d other) {
|
||||
return rotateBy(other.unaryMinus());
|
||||
}
|
||||
|
||||
/**
|
||||
* Takes the inverse of the current rotation. This is simply the negative of
|
||||
* the current angular value.
|
||||
*
|
||||
* @return The inverse of the current rotation.
|
||||
*/
|
||||
public Rotation2d unaryMinus() {
|
||||
return new Rotation2d(-m_value);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds the new rotation to the current rotation using a rotation matrix.
|
||||
*
|
||||
* <p>The matrix multiplication is as follows:
|
||||
* [cos_new] [other.cos, -other.sin][cos]
|
||||
* [sin_new] = [other.sin, other.cos][sin]
|
||||
* value_new = atan2(cos_new, sin_new)
|
||||
*
|
||||
* @param other The rotation to rotate by.
|
||||
* @return The new rotated Rotation2d.
|
||||
*/
|
||||
public Rotation2d rotateBy(Rotation2d other) {
|
||||
return new Rotation2d(
|
||||
m_cos * other.m_cos - m_sin * other.m_sin,
|
||||
m_cos * other.m_sin + m_sin * other.m_cos
|
||||
);
|
||||
}
|
||||
|
||||
/*
|
||||
* Returns the radian value of the rotation.
|
||||
*
|
||||
* @return The radian value of the rotation.
|
||||
*/
|
||||
public double getRadians() {
|
||||
return m_value;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the degree value of the rotation.
|
||||
*
|
||||
* @return The degree value of the rotation.
|
||||
*/
|
||||
public double getDegrees() {
|
||||
return Math.toDegrees(m_value);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the cosine of the rotation.
|
||||
*
|
||||
* @return The cosine of the rotation.
|
||||
*/
|
||||
public double getCos() {
|
||||
return m_cos;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the sine of the rotation.
|
||||
*
|
||||
* @return The sine of the rotation.
|
||||
*/
|
||||
public double getSin() {
|
||||
return m_sin;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the tangent of the rotation.
|
||||
*
|
||||
* @return The tangent of the rotation.
|
||||
*/
|
||||
public double getTan() {
|
||||
return m_sin / m_cos;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,69 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.geometry;
|
||||
|
||||
/**
|
||||
* Represents a transformation for a Pose2d.
|
||||
*/
|
||||
public class Transform2d {
|
||||
private final Translation2d m_translation;
|
||||
private final Rotation2d m_rotation;
|
||||
|
||||
/**
|
||||
* Constructs the transform that maps the initial pose to the final pose.
|
||||
*
|
||||
* @param initial The initial pose for the transformation.
|
||||
* @param last The final pose for the transformation.
|
||||
*/
|
||||
public Transform2d(Pose2d initial, Pose2d last) {
|
||||
// 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 = last.getTranslation().minus(initial.getTranslation())
|
||||
.rotateBy(initial.getRotation().unaryMinus());
|
||||
|
||||
m_rotation = last.getRotation().minus(initial.getRotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a transform with the given translation and rotation components.
|
||||
*
|
||||
* @param translation Translational component of the transform.
|
||||
* @param rotation Rotational component of the transform.
|
||||
*/
|
||||
public Transform2d(Translation2d translation, Rotation2d rotation) {
|
||||
m_translation = translation;
|
||||
m_rotation = rotation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs the identity transform -- maps an initial pose to itself.
|
||||
*/
|
||||
public Transform2d() {
|
||||
m_translation = new Translation2d();
|
||||
m_rotation = new Rotation2d();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the translation component of the transformation.
|
||||
*
|
||||
* @return The translational component of the transform.
|
||||
*/
|
||||
public Translation2d getTranslation() {
|
||||
return m_translation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the rotational component of the transformation.
|
||||
*
|
||||
* @return Reference to the rotational component of the transform.
|
||||
*/
|
||||
public Rotation2d getRotation() {
|
||||
return m_rotation;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,165 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.geometry;
|
||||
|
||||
/**
|
||||
* Represents a translation in 2d space.
|
||||
* This object can be used to represent a point or a vector.
|
||||
*
|
||||
* <p>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.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "MemberName"})
|
||||
public class Translation2d {
|
||||
private final double m_x;
|
||||
private final double m_y;
|
||||
|
||||
/**
|
||||
* Constructs a Translation2d with X and Y components equal to zero.
|
||||
*/
|
||||
public Translation2d() {
|
||||
this(0.0, 0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public Translation2d(double x, double y) {
|
||||
m_x = x;
|
||||
m_y = y;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the distance between two translations in 2d space.
|
||||
*
|
||||
* <p>This function uses the pythagorean theorem to calculate the distance.
|
||||
* distance = sqrt((x2 - x1)^2 + (y2 - y1)^2)
|
||||
*
|
||||
* @param other The translation to compute the distance to.
|
||||
* @return The distance between the two translations.
|
||||
*/
|
||||
public double getDistance(Translation2d other) {
|
||||
return Math.hypot(other.m_x - m_x, other.m_y - m_y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the X component of the translation.
|
||||
*
|
||||
* @return The x component of the translation.
|
||||
*/
|
||||
public double getX() {
|
||||
return m_x;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the Y component of the translation.
|
||||
*
|
||||
* @return The y component of the translation.
|
||||
*/
|
||||
public double getY() {
|
||||
return m_y;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the norm, or distance from the origin to the translation.
|
||||
*
|
||||
* @return The norm of the translation.
|
||||
*/
|
||||
public double getNorm() {
|
||||
return Math.hypot(m_x, m_y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Applies a rotation to the translation in 2d space.
|
||||
*
|
||||
* <p>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]
|
||||
*
|
||||
* <p>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.
|
||||
*/
|
||||
public Translation2d rotateBy(Rotation2d other) {
|
||||
return new Translation2d(
|
||||
m_x * other.getCos() - m_y * other.getSin(),
|
||||
m_x * other.getSin() + m_y * other.getCos()
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds two translations in 2d space and returns the sum. This is similar to
|
||||
* vector addition.
|
||||
*
|
||||
* <p>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.
|
||||
*/
|
||||
public Translation2d plus(Translation2d other) {
|
||||
return new Translation2d(m_x + other.m_x, m_y + other.m_y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Subtracts the other translation from the other translation and returns the
|
||||
* difference.
|
||||
*
|
||||
* <p>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.
|
||||
*/
|
||||
public Translation2d minus(Translation2d other) {
|
||||
return new Translation2d(m_x - other.m_x, m_y - other.m_y);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public Translation2d unaryMinus() {
|
||||
return new Translation2d(-m_x, -m_y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiplies the translation by a scalar and returns the new translation.
|
||||
*
|
||||
* <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
|
||||
*
|
||||
* @param scalar The scalar to multiply by.
|
||||
* @return The scaled translation.
|
||||
*/
|
||||
public Translation2d times(double scalar) {
|
||||
return new Translation2d(m_x * scalar, m_y * scalar);
|
||||
}
|
||||
|
||||
/**
|
||||
* Divides the translation by a scalar and returns the new translation.
|
||||
*
|
||||
* <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
|
||||
*
|
||||
* @param scalar The scalar to multiply by.
|
||||
* @return The reference to the new mutated object.
|
||||
*/
|
||||
public Translation2d div(double scalar) {
|
||||
return new Translation2d(m_x / scalar, m_y / scalar);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.geometry;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*
|
||||
* <p>A Twist can be used to represent a difference between two poses.
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
public class Twist2d {
|
||||
/**
|
||||
* Linear "dx" component.
|
||||
*/
|
||||
public double dx;
|
||||
|
||||
/**
|
||||
* Linear "dy" component.
|
||||
*/
|
||||
public double dy;
|
||||
|
||||
/**
|
||||
* Angular "dtheta" component (radians).
|
||||
*/
|
||||
public double dtheta;
|
||||
|
||||
public Twist2d() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a Twist2d with the given values.
|
||||
* @param dx Change in x direction relative to robot.
|
||||
* @param dy Change in y direction relative to robot.
|
||||
* @param dtheta Change in angle relative to robot.
|
||||
*/
|
||||
public Twist2d(double dx, double dy, double dtheta) {
|
||||
this.dx = dx;
|
||||
this.dy = dy;
|
||||
this.dtheta = dtheta;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user