mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-22 01:11:42 +00:00
[wpimath] Move Java classes to edu.wpi.first.math (#3316)
This commit is contained in:
Noam Zaks
245
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
Normal file
245
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
Normal file
@@ -0,0 +1,245 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import com.fasterxml.jackson.annotation.JsonAutoDetect;
|
||||
import com.fasterxml.jackson.annotation.JsonCreator;
|
||||
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
|
||||
import com.fasterxml.jackson.annotation.JsonProperty;
|
||||
import java.util.Objects;
|
||||
|
||||
/** Represents a 2d pose containing translational and rotational elements. */
|
||||
@JsonIgnoreProperties(ignoreUnknown = true)
|
||||
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
|
||||
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.
|
||||
*/
|
||||
@JsonCreator
|
||||
public Pose2d(
|
||||
@JsonProperty(required = true, value = "translation") Translation2d translation,
|
||||
@JsonProperty(required = true, value = "rotation") 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.
|
||||
*/
|
||||
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 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.
|
||||
*/
|
||||
public Transform2d minus(Pose2d other) {
|
||||
final var pose = this.relativeTo(other);
|
||||
return new Transform2d(pose.getTranslation(), pose.getRotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the translation component of the transformation.
|
||||
*
|
||||
* @return The translational component of the pose.
|
||||
*/
|
||||
@JsonProperty
|
||||
public Translation2d getTranslation() {
|
||||
return m_translation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the X component of the pose's translation.
|
||||
*
|
||||
* @return The x component of the pose's translation.
|
||||
*/
|
||||
public double getX() {
|
||||
return m_translation.getX();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the Y component of the pose's translation.
|
||||
*
|
||||
* @return The y component of the pose's translation.
|
||||
*/
|
||||
public double getY() {
|
||||
return m_translation.getY();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the rotational component of the transformation.
|
||||
*
|
||||
* @return The rotational component of the pose.
|
||||
*/
|
||||
@JsonProperty
|
||||
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/controls-engineering-in-frc.pdf">Controls
|
||||
* Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for a
|
||||
* 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 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.
|
||||
*/
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public Twist2d log(Pose2d end) {
|
||||
final var transform = end.relativeTo(this);
|
||||
final var dtheta = transform.getRotation().getRadians();
|
||||
final var halfDtheta = dtheta / 2.0;
|
||||
|
||||
final var cosMinusOne = transform.getRotation().getCos() - 1;
|
||||
|
||||
double halfThetaByTanOfHalfDtheta;
|
||||
if (Math.abs(cosMinusOne) < 1E-9) {
|
||||
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
|
||||
} else {
|
||||
halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
|
||||
}
|
||||
|
||||
Translation2d translationPart =
|
||||
transform
|
||||
.getTranslation()
|
||||
.rotateBy(new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta))
|
||||
.times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
|
||||
|
||||
return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks equality between this Pose2d and another object.
|
||||
*
|
||||
* @param obj The other object.
|
||||
* @return Whether the two objects are equal or not.
|
||||
*/
|
||||
@Override
|
||||
public boolean equals(Object obj) {
|
||||
if (obj instanceof Pose2d) {
|
||||
return ((Pose2d) obj).m_translation.equals(m_translation)
|
||||
&& ((Pose2d) obj).m_rotation.equals(m_rotation);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(m_translation, m_rotation);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,201 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import com.fasterxml.jackson.annotation.JsonAutoDetect;
|
||||
import com.fasterxml.jackson.annotation.JsonCreator;
|
||||
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
|
||||
import com.fasterxml.jackson.annotation.JsonProperty;
|
||||
import java.util.Objects;
|
||||
|
||||
/** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
|
||||
@JsonIgnoreProperties(ignoreUnknown = true)
|
||||
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
|
||||
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.
|
||||
*/
|
||||
@JsonCreator
|
||||
public Rotation2d(@JsonProperty(required = true, value = "radians") 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.
|
||||
*/
|
||||
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 -pi and pi.
|
||||
*
|
||||
* <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) = Rotation2d{-pi/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{-pi/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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiplies the current rotation by a scalar.
|
||||
*
|
||||
* @param scalar The scalar.
|
||||
* @return The new scaled Rotation2d.
|
||||
*/
|
||||
public Rotation2d times(double scalar) {
|
||||
return new Rotation2d(m_value * scalar);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds the new rotation to the current rotation using a rotation matrix.
|
||||
*
|
||||
* <p>The matrix multiplication is as follows:
|
||||
*
|
||||
* <pre>
|
||||
* [cos_new] [other.cos, -other.sin][cos]
|
||||
* [sin_new] = [other.sin, other.cos][sin]
|
||||
* value_new = atan2(sin_new, cos_new)
|
||||
* </pre>
|
||||
*
|
||||
* @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.
|
||||
*/
|
||||
@JsonProperty
|
||||
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;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks equality between this Rotation2d and another object.
|
||||
*
|
||||
* @param obj The other object.
|
||||
* @return Whether the two objects are equal or not.
|
||||
*/
|
||||
@Override
|
||||
public boolean equals(Object obj) {
|
||||
if (obj instanceof Rotation2d) {
|
||||
var other = (Rotation2d) obj;
|
||||
return Math.hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(m_value);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,133 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
/** 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();
|
||||
}
|
||||
|
||||
/**
|
||||
* Scales the transform by the scalar.
|
||||
*
|
||||
* @param scalar The scalar.
|
||||
* @return The scaled Transform2d.
|
||||
*/
|
||||
public Transform2d times(double scalar) {
|
||||
return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the translation component of the transformation.
|
||||
*
|
||||
* @return The translational component of the transform.
|
||||
*/
|
||||
public Translation2d getTranslation() {
|
||||
return m_translation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the X component of the transformation's translation.
|
||||
*
|
||||
* @return The x component of the transformation's translation.
|
||||
*/
|
||||
public double getX() {
|
||||
return m_translation.getX();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the Y component of the transformation's translation.
|
||||
*
|
||||
* @return The y component of the transformation's translation.
|
||||
*/
|
||||
public double getY() {
|
||||
return m_translation.getY();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the rotational component of the transformation.
|
||||
*
|
||||
* @return Reference to the rotational component of the transform.
|
||||
*/
|
||||
public Rotation2d getRotation() {
|
||||
return m_rotation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Invert the transformation. This is useful for undoing a transformation.
|
||||
*
|
||||
* @return The inverted transformation.
|
||||
*/
|
||||
public Transform2d inverse() {
|
||||
// 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 new Transform2d(
|
||||
getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
|
||||
getRotation().unaryMinus());
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks equality between this Transform2d and another object.
|
||||
*
|
||||
* @param obj The other object.
|
||||
* @return Whether the two objects are equal or not.
|
||||
*/
|
||||
@Override
|
||||
public boolean equals(Object obj) {
|
||||
if (obj instanceof Transform2d) {
|
||||
return ((Transform2d) obj).m_translation.equals(m_translation)
|
||||
&& ((Transform2d) obj).m_rotation.equals(m_rotation);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(m_translation, m_rotation);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,199 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import com.fasterxml.jackson.annotation.JsonAutoDetect;
|
||||
import com.fasterxml.jackson.annotation.JsonCreator;
|
||||
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
|
||||
import com.fasterxml.jackson.annotation.JsonProperty;
|
||||
import java.util.Objects;
|
||||
|
||||
/**
|
||||
* 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"})
|
||||
@JsonIgnoreProperties(ignoreUnknown = true)
|
||||
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
|
||||
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.
|
||||
*/
|
||||
@JsonCreator
|
||||
public Translation2d(
|
||||
@JsonProperty(required = true, value = "x") double x,
|
||||
@JsonProperty(required = true, value = "y") double y) {
|
||||
m_x = x;
|
||||
m_y = y;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a Translation2d with the provided distance and angle. This is essentially converting
|
||||
* from polar coordinates to Cartesian coordinates.
|
||||
*
|
||||
* @param distance The distance from the origin to the end of the translation.
|
||||
* @param angle The angle between the x-axis and the translation vector.
|
||||
*/
|
||||
public Translation2d(double distance, Rotation2d angle) {
|
||||
m_x = distance * angle.getCos();
|
||||
m_y = distance * angle.getSin();
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
@JsonProperty
|
||||
public double getX() {
|
||||
return m_x;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the Y component of the translation.
|
||||
*
|
||||
* @return The y component of the translation.
|
||||
*/
|
||||
@JsonProperty
|
||||
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);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks equality between this Translation2d and another object.
|
||||
*
|
||||
* @param obj The other object.
|
||||
* @return Whether the two objects are equal or not.
|
||||
*/
|
||||
@Override
|
||||
public boolean equals(Object obj) {
|
||||
if (obj instanceof Translation2d) {
|
||||
return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
|
||||
&& Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(m_x, m_y);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.geometry;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks equality between this Twist2d and another object.
|
||||
*
|
||||
* @param obj The other object.
|
||||
* @return Whether the two objects are equal or not.
|
||||
*/
|
||||
@Override
|
||||
public boolean equals(Object obj) {
|
||||
if (obj instanceof Twist2d) {
|
||||
return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
|
||||
&& Math.abs(((Twist2d) obj).dy - dy) < 1E-9
|
||||
&& Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(dx, dy, dtheta);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user