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

[wpimath] Add 3D geometry classes (#4175)

Browse Source 
Also clean up 2D geometry documentation.
This commit is contained in:
Tyler Veness
2022-05-06 08:41:23 -07:00
committed by GitHub
parent 708a4bc3bc
commit f20a20f3f1
48 changed files with 4299 additions and 255 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View File

@@ -11,17 +11,14 @@ import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/** Represents a 2d pose containing translational and rotational elements. */
/** Represents a 2D pose containing translational and rotational elements. */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Pose2d implements Interpolatable<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})
*/
/** Constructs a pose at the origin facing toward the positive X axis. */
public Pose2d() {
m_translation = new Translation2d();
m_rotation = new Rotation2d();
@@ -42,8 +39,7 @@ public class Pose2d implements Interpolatable<Pose2d> {
}
/**
* Convenience constructors that takes in x and y values directly instead of having to construct a
* Translation2d.
* Constructs a pose with x and y translations instead of a separate 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.
@@ -57,8 +53,11 @@ public class Pose2d implements Interpolatable<Pose2d> {
/**
* 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]
* <pre>
* [x_new] [cos, -sin, 0][transform.x]
* [y_new] += [sin, cos, 0][transform.y]
* [t_new] [ 0, 0, 1][transform.t]
* </pre>
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
@@ -160,8 +159,8 @@ public class Pose2d implements Interpolatable<Pose2d> {
*
* @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)}
* degrees since the previous pose update, the twist would be Twist2d(0.01, 0.0,
* Units.degreesToRadians(0.5)).
* @return The new pose of the robot.
*/
public Pose2d exp(Twist2d twist) {

326
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java Normal file
View File

@@ -0,0 +1,326 @@
// 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 edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
/** Represents a 3D pose containing translational and rotational elements. */
public class Pose3d implements Interpolatable<Pose3d> {
private final Translation3d m_translation;
private final Rotation3d m_rotation;
/** Constructs a pose at the origin facing toward the positive X axis. */
public Pose3d() {
m_translation = new Translation3d();
m_rotation = new Rotation3d();
}
/**
* 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 Pose3d(Translation3d translation, Rotation3d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/**
* Constructs a pose with x, y, and z translations instead of a separate Translation3d.
*
* @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 z The z component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
public Pose3d(double x, double y, double z, Rotation3d rotation) {
m_translation = new Translation3d(x, y, z);
m_rotation = rotation;
}
/**
* Transforms the pose by the given transformation and returns the new transformed pose.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose3d plus(Transform3d other) {
return transformBy(other);
}
/**
* Returns the Transform3d 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 Transform3d minus(Pose3d other) {
final var pose = this.relativeTo(other);
return new Transform3d(pose.getTranslation(), pose.getRotation());
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the pose.
*/
public Translation3d 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 Z component of the pose's translation.
*
* @return The z component of the pose's translation.
*/
public double getZ() {
return m_translation.getZ();
}
/**
* Returns the rotational component of the transformation.
*
* @return The rotational component of the pose.
*/
public Rotation3d 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 Pose3d transformBy(Transform3d other) {
return new Pose3d(
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 Pose3d relativeTo(Pose3d other) {
var transform = new Transform3d(other, this);
return new Pose3d(transform.getTranslation(), transform.getRotation());
}
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*
* <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 Twist3d(0.01, 0.0, 0.0, new new
* Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))).
* @return The new pose of the robot.
*/
@SuppressWarnings("LocalVariableName")
public Pose3d exp(Twist3d twist) {
final var Omega = rotationVectorToMatrix(VecBuilder.fill(twist.rx, twist.ry, twist.rz));
final var OmegaSq = Omega.times(Omega);
double thetaSq = twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz;
// Get left Jacobian of SO3. See first line in right column of
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
Matrix<N3, N3> J;
if (thetaSq < 1E-9 * 1E-9) {
// J = I + 0.5ω
J = Matrix.eye(Nat.N3()).plus(Omega.times(0.5));
} else {
double theta = Math.sqrt(thetaSq);
// J = I + (1 − cos(θ))/θ² ω + (θ − sin(θ))/θ³ ω²
J =
Matrix.eye(Nat.N3())
.plus(Omega.times((1.0 - Math.cos(theta)) / thetaSq))
.plus(OmegaSq.times((theta - Math.sin(theta)) / (thetaSq * theta)));
}
// Get translation component
final var translation =
J.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(twist.dx, twist.dy, twist.dz));
final var transform =
new Transform3d(
new Translation3d(translation.get(0, 0), translation.get(1, 0), translation.get(2, 0)),
new Rotation3d(twist.rx, twist.ry, twist.rz));
return this.plus(transform);
}
/**
* Returns a Twist3d 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.
*/
@SuppressWarnings("LocalVariableName")
public Twist3d log(Pose3d end) {
final var transform = end.relativeTo(this);
final var rotVec = transform.getRotation().getQuaternion().toRotationVector();
final var Omega = rotationVectorToMatrix(rotVec);
final var OmegaSq = Omega.times(Omega);
double thetaSq =
rotVec.get(0, 0) * rotVec.get(0, 0)
+ rotVec.get(1, 0) * rotVec.get(1, 0)
+ rotVec.get(2, 0) * rotVec.get(2, 0);
// Get left Jacobian inverse of SO3. See fourth line in right column of
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
Matrix<N3, N3> Jinv;
if (thetaSq < 1E-9 * 1E-9) {
// J⁻¹ = I − 0.5ω + 1/12 ω²
Jinv = Matrix.eye(Nat.N3()).minus(Omega.times(0.5)).plus(OmegaSq.times(1.0 / 12.0));
} else {
double theta = Math.sqrt(thetaSq);
double halfTheta = 0.5 * theta;
// J⁻¹ = I − 0.5ω + (1 − 0.5θ cos(θ/2) / sin(θ/2))/θ² ω²
Jinv =
Matrix.eye(Nat.N3())
.minus(Omega.times(0.5))
.plus(
OmegaSq.times(
(1.0 - 0.5 * theta * Math.cos(halfTheta) / Math.sin(halfTheta)) / thetaSq));
}
// Get dtranslation component
final var dtranslation =
Jinv.times(
new MatBuilder<>(Nat.N3(), Nat.N1())
.fill(transform.getX(), transform.getY(), transform.getZ()));
return new Twist3d(
dtranslation.get(0, 0),
dtranslation.get(1, 0),
dtranslation.get(2, 0),
rotVec.get(0, 0),
rotVec.get(1, 0),
rotVec.get(2, 0));
}
/**
* Returns a Pose2d representing this Pose3d projected into the X-Y plane.
*
* @return A Pose2d representing this Pose3d projected into the X-Y plane.
*/
public Pose2d toPose2d() {
return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d());
}
@Override
public String toString() {
return String.format("Pose3d(%s, %s)", m_translation, m_rotation);
}
/**
* Checks equality between this Pose3d 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 Pose3d) {
return ((Pose3d) obj).m_translation.equals(m_translation)
&& ((Pose3d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
@Override
@SuppressWarnings("ParameterName")
public Pose3d interpolate(Pose3d endValue, double t) {
if (t < 0) {
return this;
} else if (t >= 1) {
return endValue;
} else {
var twist = this.log(endValue);
var scaledTwist =
new Twist3d(
twist.dx * t, twist.dy * t, twist.dz * t, twist.rx * t, twist.ry * t, twist.rz * t);
return this.exp(scaledTwist);
}
}
/**
* Applies the hat operator to a rotation vector.
*
* <p>It takes a rotation vector and returns the corresponding matrix representation of the Lie
* algebra element (a 3x3 rotation matrix).
*
* @param rotation The rotation vector.
* @return The rotation vector as a 3x3 rotation matrix.
*/
private Matrix<N3, N3> rotationVectorToMatrix(Vector<N3> rotation) {
// Given a rotation vector <a, b, c>,
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
return new MatBuilder<>(Nat.N3(), Nat.N3())
.fill(
0.0,
-rotation.get(2, 0),
rotation.get(1, 0),
rotation.get(2, 0),
0.0,
-rotation.get(0, 0),
-rotation.get(1, 0),
rotation.get(0, 0),
0.0);
}
}

187
wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java Normal file
View File

@@ -0,0 +1,187 @@
// 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 edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
public class Quaternion {
private final double m_r;
private final Vector<N3> m_v;
/** Constructs a quaternion with a default angle of 0 degrees. */
public Quaternion() {
m_r = 1.0;
m_v = VecBuilder.fill(0.0, 0.0, 0.0);
}
/**
* Constructs a quaternion with the given components.
*
* @param w W component of the quaternion.
* @param x X component of the quaternion.
* @param y Y component of the quaternion.
* @param z Z component of the quaternion.
*/
public Quaternion(double w, double x, double y, double z) {
m_r = w;
m_v = VecBuilder.fill(x, y, z);
}
/**
* Multiply with another quaternion.
*
* @param other The other quaternion.
* @return The quaternion product.
*/
public Quaternion times(Quaternion other) {
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
final var r1 = m_r;
final var v1 = m_v;
final var r2 = other.m_r;
final var v2 = other.m_v;
final var v1x = v1.get(0, 0);
final var v1y = v1.get(1, 0);
final var v1z = v1.get(2, 0);
final var v2x = v2.get(0, 0);
final var v2y = v2.get(1, 0);
final var v2z = v2.get(2, 0);
var cross =
VecBuilder.fill(v1y * v2z - v2y * v1z, v2x * v1z - v1x * v2z, v1x * v2y - v2x * v1y);
double dot = v1x * v2x + v1y * v2y + v1z * v2z;
final var v = v2.times(r1).plus(v1.times(r2)).plus(cross);
return new Quaternion(r1 * r2 - dot, v.get(0, 0), v.get(1, 0), v.get(2, 0));
}
@Override
public String toString() {
return String.format(
"Quaternion(%s, %s, %s, %s)", m_r, m_v.get(0, 0), m_v.get(1, 0), m_v.get(2, 0));
}
/**
* Checks equality between this Quaternion 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 Quaternion) {
var other = (Quaternion) obj;
final var r1 = m_r;
final var v1 = m_v;
final var r2 = other.m_r;
final var v2 = other.m_v;
final var v1x = v1.get(0, 0);
final var v1y = v1.get(1, 0);
final var v1z = v1.get(2, 0);
final var v2x = v2.get(0, 0);
final var v2y = v2.get(1, 0);
final var v2z = v2.get(2, 0);
return Math.abs(r1 * r2 + v1x * v2x + v1y * v2y + v1z * v2z) > 1.0 - 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_r, m_v);
}
/**
* Returns the inverse of the quaternion.
*
* @return The inverse quaternion.
*/
public Quaternion inverse() {
return new Quaternion(m_r, -m_v.get(0, 0), -m_v.get(1, 0), -m_v.get(2, 0));
}
/**
* Normalizes the quaternion.
*
* @return The normalized quaternion.
*/
public Quaternion normalize() {
double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ());
if (norm == 0.0) {
return new Quaternion();
} else {
return new Quaternion(getW() / norm, getX() / norm, getY() / norm, getZ() / norm);
}
}
/**
* Returns W component of the quaternion.
*
* @return W component of the quaternion.
*/
public double getW() {
return m_r;
}
/**
* Returns X component of the quaternion.
*
* @return X component of the quaternion.
*/
public double getX() {
return m_v.get(0, 0);
}
/**
* Returns Y component of the quaternion.
*
* @return Y component of the quaternion.
*/
public double getY() {
return m_v.get(1, 0);
}
/**
* Returns Z component of the quaternion.
*
* @return Z component of the quaternion.
*/
public double getZ() {
return m_v.get(2, 0);
}
/**
* Returns the rotation vector representation of this quaternion.
*
* <p>This is also the log operator of SO(3).
*
* @return The rotation vector representation of this quaternion.
*/
public Vector<N3> toRotationVector() {
// See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
// Sound State Representation through Encapsulation of Manifolds"
//
// https://arxiv.org/pdf/1107.1119.pdf
double norm = m_v.normF();
if (norm < 1e-9) {
return m_v.times(2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW()));
} else {
if (getW() < 0.0) {
return m_v.times(2.0 * Math.atan2(-norm, -getW()) / norm);
} else {
return m_v.times(2.0 * Math.atan2(norm, getW()) / norm);
}
}
}
}

4
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
View File

@@ -13,7 +13,7 @@ import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.util.Units;
import java.util.Objects;
/** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
/** 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 implements Interpolatable<Rotation2d> {
@@ -29,7 +29,7 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
}
/**
* Constructs a Rotation2d with the given radian value. The x and y don't have to be normalized.
* Constructs a Rotation2d with the given radian value.
*
* @param value The value of the angle in radians.
*/

259
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java Normal file
View File

@@ -0,0 +1,259 @@
// 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 edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
/** A rotation in a 3D coordinate. */
public class Rotation3d implements Interpolatable<Rotation3d> {
private Quaternion m_q = new Quaternion();
/** Constructs a Rotation3d with a default angle of 0 degrees. */
public Rotation3d() {}
/**
* Constructs a Rotation3d from a quaternion.
*
* @param q The quaternion.
*/
public Rotation3d(Quaternion q) {
m_q = q.normalize();
}
/**
* Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
*
* <p>Extrinsic rotations occur in that order around the axes in the fixed global frame rather
* than the body frame.
*
* @param roll The counterclockwise rotation angle around the X axis (roll) in radians.
* @param pitch The counterclockwise rotation angle around the Y axis (pitch) in radians.
* @param yaw The counterclockwise rotation angle around the Z axis (yaw) in radians.
*/
public Rotation3d(double roll, double pitch, double yaw) {
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
double cr = Math.cos(roll * 0.5);
double sr = Math.sin(roll * 0.5);
double cp = Math.cos(pitch * 0.5);
double sp = Math.sin(pitch * 0.5);
double cy = Math.cos(yaw * 0.5);
double sy = Math.sin(yaw * 0.5);
m_q =
new Quaternion(
cr * cp * cy + sr * sp * sy,
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy);
}
/**
* Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be
* normalized.
*
* @param axis The rotation axis.
* @param angleRadians The rotation around the axis in radians.
*/
public Rotation3d(Vector<N3> axis, double angleRadians) {
double norm = axis.normF();
if (norm == 0.0) {
return;
}
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
var v = axis.times(1.0 / norm).times(Math.sin(angleRadians / 2.0));
m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0));
}
/**
* Adds two rotations together.
*
* @param other The rotation to add.
* @return The sum of the two rotations.
*/
public Rotation3d plus(Rotation3d other) {
return rotateBy(other);
}
/**
* Subtracts the new rotation from the current rotation and returns the new rotation.
*
* @param other The rotation to subtract.
* @return The difference between the two rotations.
*/
public Rotation3d minus(Rotation3d other) {
return rotateBy(other.unaryMinus());
}
/**
* Takes the inverse of the current rotation.
*
* @return The inverse of the current rotation.
*/
public Rotation3d unaryMinus() {
return new Rotation3d(m_q.inverse());
}
/**
* Multiplies the current rotation by a scalar.
*
* @param scalar The scalar.
* @return The new scaled Rotation3d.
*/
public Rotation3d times(double scalar) {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
if (m_q.getW() >= 0.0) {
return new Rotation3d(
VecBuilder.fill(m_q.getX(), m_q.getY(), m_q.getZ()),
2.0 * scalar * Math.acos(m_q.getW()));
} else {
return new Rotation3d(
VecBuilder.fill(-m_q.getX(), -m_q.getY(), -m_q.getZ()),
2.0 * scalar * Math.acos(-m_q.getW()));
}
}
/**
* Adds the new rotation to the current rotation.
*
* @param other The rotation to rotate by.
* @return The new rotated Rotation3d.
*/
public Rotation3d rotateBy(Rotation3d other) {
return new Rotation3d(other.m_q.times(m_q));
}
/**
* Returns the quaternion representation of the Rotation3d.
*
* @return The quaternion representation of the Rotation3d.
*/
public Quaternion getQuaternion() {
return m_q;
}
/**
* Returns the counterclockwise rotation angle around the X axis (roll) in radians.
*
* @return The counterclockwise rotation angle around the X axis (roll) in radians.
*/
public double getX() {
final var w = m_q.getW();
final var x = m_q.getX();
final var y = m_q.getY();
final var z = m_q.getZ();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
return Math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
}
/**
* Returns the counterclockwise rotation angle around the Y axis (pitch) in radians.
*
* @return The counterclockwise rotation angle around the Y axis (pitch) in radians.
*/
public double getY() {
final var w = m_q.getW();
final var x = m_q.getX();
final var y = m_q.getY();
final var z = m_q.getZ();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
double ratio = 2.0 * (w * y - z * x);
if (Math.abs(ratio) >= 1.0) {
return Math.copySign(Math.PI / 2.0, ratio);
} else {
return Math.asin(ratio);
}
}
/**
* Returns the counterclockwise rotation angle around the Z axis (yaw) in radians.
*
* @return The counterclockwise rotation angle around the Z axis (yaw) in radians.
*/
public double getZ() {
final var w = m_q.getW();
final var x = m_q.getX();
final var y = m_q.getY();
final var z = m_q.getZ();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
return Math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
}
/**
* Returns the axis in the axis-angle representation of this rotation.
*
* @return The axis in the axis-angle representation.
*/
public Vector<N3> getAxis() {
double norm =
Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
if (norm == 0.0) {
return VecBuilder.fill(0.0, 0.0, 0.0);
} else {
return VecBuilder.fill(m_q.getX() / norm, m_q.getY() / norm, m_q.getZ() / norm);
}
}
/**
* Returns the angle in radians in the axis-angle representation of this rotation.
*
* @return The angle in radians in the axis-angle representation of this rotation.
*/
public double getAngle() {
double norm =
Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
return 2.0 * Math.atan2(norm, m_q.getW());
}
/**
* Returns a Rotation2d representing this Rotation3d projected into the X-Y plane.
*
* @return A Rotation2d representing this Rotation3d projected into the X-Y plane.
*/
public Rotation2d toRotation2d() {
return new Rotation2d(getZ());
}
@Override
public String toString() {
return String.format("Rotation3d(%s)", m_q);
}
/**
* Checks equality between this Rotation3d 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 Rotation3d) {
var other = (Rotation3d) obj;
return m_q.equals(other.m_q);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_q);
}
@Override
@SuppressWarnings("ParameterName")
public Rotation3d interpolate(Rotation3d endValue, double t) {
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}
}

152
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java Normal file
View File

@@ -0,0 +1,152 @@
// 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 Pose3d. */
public class Transform3d {
private final Translation3d m_translation;
private final Rotation3d 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 Transform3d(Pose3d initial, Pose3d 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 Transform3d(Translation3d translation, Rotation3d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/** Constructs the identity transform -- maps an initial pose to itself. */
public Transform3d() {
m_translation = new Translation3d();
m_rotation = new Rotation3d();
}
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform3d.
*/
public Transform3d times(double scalar) {
return new Transform3d(m_translation.times(scalar), m_rotation.times(scalar));
}
/**
* Composes two transformations.
*
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
*/
public Transform3d plus(Transform3d other) {
return new Transform3d(new Pose3d(), new Pose3d().transformBy(this).transformBy(other));
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the transform.
*/
public Translation3d 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 Z component of the transformation's translation.
*
* @return The z component of the transformation's translation.
*/
public double getZ() {
return m_translation.getZ();
}
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
public Rotation3d getRotation() {
return m_rotation;
}
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
public Transform3d 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 Transform3d(
getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
getRotation().unaryMinus());
}
@Override
public String toString() {
return String.format("Transform3d(%s, %s)", m_translation, m_rotation);
}
/**
* Checks equality between this Transform3d 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 Transform3d) {
return ((Transform3d) obj).m_translation.equals(m_translation)
&& ((Transform3d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
}

47
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
View File

@@ -13,11 +13,10 @@ import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/**
* Represents a translation in 2d space. This object can be used to represent a point or a vector.
* 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.
* <p>This assumes that you are using conventional mathematical axes. When the robot is at the
* origin facing in the positive X direction, forward is positive X and left is positive Y.
*/
@SuppressWarnings({"ParameterName", "MemberName"})
@JsonIgnoreProperties(ignoreUnknown = true)
@@ -58,10 +57,9 @@ public class Translation2d implements Interpolatable<Translation2d> {
}
/**
* Calculates the distance between two translations in 2d space.
* 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)
* <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²).
*
* @param other The translation to compute the distance to.
* @return The distance between the two translations.
@@ -73,7 +71,7 @@ public class Translation2d implements Interpolatable<Translation2d> {
/**
* Returns the X component of the translation.
*
* @return The x component of the translation.
* @return The X component of the translation.
*/
@JsonProperty
public double getX() {
@@ -83,7 +81,7 @@ public class Translation2d implements Interpolatable<Translation2d> {
/**
* Returns the Y component of the translation.
*
* @return The y component of the translation.
* @return The Y component of the translation.
*/
@JsonProperty
public double getY() {
@@ -100,13 +98,18 @@ public class Translation2d implements Interpolatable<Translation2d> {
}
/**
* Applies a rotation to the translation in 2d space.
* 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]
* angle.
*
* <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a Translation2d of
* {0, 2}.
* <pre>
* [x_new] [other.cos, -other.sin][x]
* [y_new] = [other.sin, other.cos][y]
* </pre>
*
* <p>For example, rotating a Translation2d of &lt;2, 0&gt; by 90 degrees will return a
* Translation2d of &lt;0, 2&gt;.
*
* @param other The rotation to rotate the translation by.
* @return The new rotated translation.
@@ -117,9 +120,9 @@ public class Translation2d implements Interpolatable<Translation2d> {
}
/**
* Adds two translations in 2d space and returns the sum. This is similar to vector addition.
* Returns the sum of two translations in 2D space.
*
* <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} = Translation2d{3.0, 8.0}
* <p>For example, Translation3d(1.0, 2.5) + Translation3d(2.0, 5.5) = Translation3d{3.0, 8.0).
*
* @param other The translation to add.
* @return The sum of the translations.
@@ -129,9 +132,9 @@ public class Translation2d implements Interpolatable<Translation2d> {
}
/**
* Subtracts the other translation from the other translation and returns the difference.
* Returns the difference between two translations.
*
* <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} = Translation2d{4.0, 2.0}
* <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.
@@ -142,7 +145,7 @@ public class Translation2d implements Interpolatable<Translation2d> {
/**
* 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.
* flipping the point over both axes, or negating all components of the translation.
*
* @return The inverse of the current translation.
*/
@@ -151,9 +154,9 @@ public class Translation2d implements Interpolatable<Translation2d> {
}
/**
* Multiplies the translation by a scalar and returns the new translation.
* Returns the translation multiplied by a scalar.
*
* <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
* <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.
@@ -163,9 +166,9 @@ public class Translation2d implements Interpolatable<Translation2d> {
}
/**
* Divides the translation by a scalar and returns the new translation.
* Returns the translation divided by a scalar.
*
* <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
* <p>For example, Translation3d(2.0, 2.5) / 2 = Translation3d(1.0, 1.25).
*
* @param scalar The scalar to multiply by.
* @return The reference to the new mutated object.

222
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java Normal file
View File

@@ -0,0 +1,222 @@
// 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 edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/**
* Represents a translation in 3D 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 at the
* origin facing in the positive X direction, forward is positive X, left is positive Y, and up is
* positive Z.
*/
@SuppressWarnings({"ParameterName", "MemberName"})
public class Translation3d implements Interpolatable<Translation3d> {
private final double m_x;
private final double m_y;
private final double m_z;
/** Constructs a Translation3d with X, Y, and Z components equal to zero. */
public Translation3d() {
this(0.0, 0.0, 0.0);
}
/**
* Constructs a Translation3d with the X, Y, and Z components equal to the provided values.
*
* @param x The x component of the translation.
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
public Translation3d(double x, double y, double z) {
m_x = x;
m_y = y;
m_z = z;
}
/**
* Constructs a Translation3d 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 Translation3d(double distance, Rotation3d angle) {
final var rectangular = new Translation3d(distance, 0.0, 0.0).rotateBy(angle);
m_x = rectangular.getX();
m_y = rectangular.getY();
m_z = rectangular.getZ();
}
/**
* Calculates the distance between two translations in 3D space.
*
* <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
*
* @param other The translation to compute the distance to.
* @return The distance between the two translations.
*/
public double getDistance(Translation3d other) {
return Math.sqrt(
Math.pow(other.m_x - m_x, 2) + Math.pow(other.m_y - m_y, 2) + Math.pow(other.m_z - m_z, 2));
}
/**
* 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 Z component of the translation.
*
* @return The Z component of the translation.
*/
public double getZ() {
return m_z;
}
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
public double getNorm() {
return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
/**
* Applies a rotation to the translation in 3D space.
*
* <p>For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees around the Z axis
* will return a Translation3d of &lt;0, 2, 0&gt;.
*
* @param other The rotation to rotate the translation by.
* @return The new rotated translation.
*/
public Translation3d rotateBy(Rotation3d other) {
final var p = new Quaternion(0.0, m_x, m_y, m_z);
final var qprime = other.getQuaternion().times(p).times(other.getQuaternion().inverse());
return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
}
/**
* Returns a Translation2d representing this Translation3d projected into the X-Y plane.
*
* @return A Translation2d representing this Translation3d projected into the X-Y plane.
*/
public Translation2d toTranslation2d() {
return new Translation2d(m_x, m_y);
}
/**
* Returns the sum of two translations in 3D space.
*
* <p>For example, Translation3d(1.0, 2.5, 3.5) + Translation3d(2.0, 5.5, 7.5) =
* Translation3d{3.0, 8.0, 11.0).
*
* @param other The translation to add.
* @return The sum of the translations.
*/
public Translation3d plus(Translation3d other) {
return new Translation3d(m_x + other.m_x, m_y + other.m_y, m_z + other.m_z);
}
/**
* Returns the difference between two translations.
*
* <p>For example, Translation3d(5.0, 4.0, 3.0) - Translation3d(1.0, 2.0, 3.0) =
* Translation3d(4.0, 2.0, 0.0).
*
* @param other The translation to subtract.
* @return The difference between the two translations.
*/
public Translation3d minus(Translation3d other) {
return new Translation3d(m_x - other.m_x, m_y - other.m_y, m_z - other.m_z);
}
/**
* Returns the inverse of the current translation. This is equivalent to negating all components
* of the translation.
*
* @return The inverse of the current translation.
*/
public Translation3d unaryMinus() {
return new Translation3d(-m_x, -m_y, -m_z);
}
/**
* Returns the translation multiplied by a scalar.
*
* <p>For example, Translation3d(2.0, 2.5, 4.5) * 2 = Translation3d(4.0, 5.0, 9.0).
*
* @param scalar The scalar to multiply by.
* @return The scaled translation.
*/
public Translation3d times(double scalar) {
return new Translation3d(m_x * scalar, m_y * scalar, m_z * scalar);
}
/**
* Returns the translation divided by a scalar.
*
* <p>For example, Translation3d(2.0, 2.5, 4.5) / 2 = Translation3d(1.0, 1.25, 2.25).
*
* @param scalar The scalar to multiply by.
* @return The reference to the new mutated object.
*/
public Translation3d div(double scalar) {
return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar);
}
@Override
public String toString() {
return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z);
}
/**
* Checks equality between this Translation3d 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 Translation3d) {
return Math.abs(((Translation3d) obj).m_x - m_x) < 1E-9
&& Math.abs(((Translation3d) obj).m_y - m_y) < 1E-9
&& Math.abs(((Translation3d) obj).m_z - m_z) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_x, m_y, m_z);
}
@Override
public Translation3d interpolate(Translation3d endValue, double t) {
return new Translation3d(
MathUtil.interpolate(this.getX(), endValue.getX(), t),
MathUtil.interpolate(this.getY(), endValue.getY(), t),
MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
}
}

4
wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
View File

@@ -7,8 +7,8 @@ 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.
* A change in distance along a 2D 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.
*/

86
wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java Normal file
View File

@@ -0,0 +1,86 @@
// 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 a 3D arc since the last pose update. We can use ideas from
* differential calculus to create new Pose3ds from a Twist3d and vise versa.
*
* <p>A Twist can be used to represent a difference between two poses.
*/
@SuppressWarnings("MemberName")
public class Twist3d {
/** Linear "dx" component. */
public double dx;
/** Linear "dy" component. */
public double dy;
/** Linear "dz" component. */
public double dz;
/** Rotation vector x component (radians). */
public double rx;
/** Rotation vector y component (radians). */
public double ry;
/** Rotation vector z component (radians). */
public double rz;
public Twist3d() {}
/**
* Constructs a Twist3d with the given values.
*
* @param dx Change in x direction relative to robot.
* @param dy Change in y direction relative to robot.
* @param dz Change in z direction relative to robot.
* @param rx Rotation vector x component.
* @param ry Rotation vector y component.
* @param rz Rotation vector z component.
*/
public Twist3d(double dx, double dy, double dz, double rx, double ry, double rz) {
this.dx = dx;
this.dy = dy;
this.dz = dz;
this.rx = rx;
this.ry = ry;
this.rz = rz;
}
@Override
public String toString() {
return String.format(
"Twist3d(dX: %.2f, dY: %.2f, dZ: %.2f, rX: %.2f, rY: %.2f, rZ: %.2f)",
dx, dy, dz, rx, ry, rz);
}
/**
* Checks equality between this Twist3d 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 Twist3d) {
return Math.abs(((Twist3d) obj).dx - dx) < 1E-9
&& Math.abs(((Twist3d) obj).dy - dy) < 1E-9
&& Math.abs(((Twist3d) obj).dz - dz) < 1E-9
&& Math.abs(((Twist3d) obj).rx - rx) < 1E-9
&& Math.abs(((Twist3d) obj).ry - ry) < 1E-9
&& Math.abs(((Twist3d) obj).rz - rz) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(dx, dy, dz, rx, ry, rz);
}
}