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

[wpimath] Add Rotation3d rotation vector getter (#7564)

Browse Source 
The code churn in Java is just making the function order consistent
between languages.
This commit is contained in:
Tyler Veness
2024-12-22 13:34:51 -08:00
committed by GitHub
parent 0c99073b94
commit 02a0adc653
5 changed files with 40 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -459,6 +459,32 @@ public class Rotation3d
return Radians.of(getZ());
}
/**
* 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 rotation matrix representation of this rotation.
*
@@ -486,29 +512,12 @@ public class Rotation3d
}
/**
* Returns the axis in the axis-angle representation of this rotation.
* Returns rotation vector representation of this rotation.
*
* @return The axis in the axis-angle representation.
* @return Rotation vector representation of this rotation.
*/
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());
public Vector<N3> toVector() {
return m_q.toRotationVector();
}
/**

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java
View File

@@ -125,7 +125,7 @@ public class Odometry3d<T> {
*/
public Pose3d update(Rotation3d gyroAngle, T wheelPositions) {
var angle = gyroAngle.plus(m_gyroOffset);
var angle_difference = angle.minus(m_previousAngle).getQuaternion().toRotationVector();
var angle_difference = angle.minus(m_previousAngle).toVector();
var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
var twist =

2
wpimath/src/main/native/include/frc/geometry/Pose3d.h
View File

@@ -404,7 +404,7 @@ constexpr Twist3d Pose3d::Log(const Pose3d& end) const {
Vector3d u{
{transform.X().value(), transform.Y().value(), transform.Z().value()}};
Vector3d rvec = transform.Rotation().GetQuaternion().ToRotationVector();
Vector3d rvec = transform.Rotation().ToVector();
Matrix3d omega = detail::RotationVectorToMatrix(rvec);
Matrix3d omegaSq = omega * omega;
double theta = rvec.norm();

7
wpimath/src/main/native/include/frc/geometry/Rotation3d.h
View File

@@ -421,6 +421,13 @@ class WPILIB_DLLEXPORT Rotation3d {
1.0 - 2.0 * (x * x + y * y)}};
}
/**
* Returns rotation vector representation of this rotation.
*
* @return Rotation vector representation of this rotation.
*/
constexpr Eigen::Vector3d ToVector() const { return m_q.ToRotationVector(); }
/**
* Returns a Rotation2d representing this Rotation3d projected into the X-Y
* plane.

3
wpimath/src/main/native/include/frc/kinematics/Odometry3d.h
View File

@@ -120,8 +120,7 @@ class WPILIB_DLLEXPORT Odometry3d {
const Pose3d& Update(const Rotation3d& gyroAngle,
const WheelPositions& wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
auto angle_difference =
(angle - m_previousAngle).GetQuaternion().ToRotationVector();
auto angle_difference = (angle - m_previousAngle).ToVector();
auto twist2d =
m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);