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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-01-16 23:17:59 -08:00
parent 1cad4f64a4 a9f3fc6b2c
commit d9f8fded09
92 changed files with 2748 additions and 534 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -238,6 +238,17 @@ public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, Str
return new Pose2d(transform.getTranslation(), transform.getRotation());
}
/**
* Rotates the current pose around a point in 2D space.
*
* @param point The point in 2D space to rotate around.
* @param rot The rotation to rotate the pose by.
* @return The new rotated pose.
*/
public Pose2d rotateAround(Translation2d point, Rotation2d rot) {
return new Pose2d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
}
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
*

11
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -271,6 +271,17 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
return new Pose3d(transform.getTranslation(), transform.getRotation());
}
/**
* Rotates the current pose around a point in 3D space.
*
* @param point The point in 3D space to rotate around.
* @param rot The rotation to rotate the pose by.
* @return The new rotated pose.
*/
public Pose3d rotateAround(Translation3d point, Rotation3d rot) {
return new Pose3d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
}
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*

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

@@ -110,11 +110,11 @@ public class Rotation2d
public Rotation2d(double x, double y) {
double magnitude = Math.hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
m_sin = y / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
m_sin = 0.0;
MathSharedStore.reportError(
"x and y components of Rotation2d are zero\n", Thread.currentThread().getStackTrace());
}

11
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -216,6 +216,17 @@ public class Translation3d
return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
}
/**
* Rotates this translation around another translation in 3D space.
*
* @param other The other translation to rotate around.
* @param rot The rotation to rotate the translation by.
* @return The new rotated translation.
*/
public Translation3d rotateAround(Translation3d other, Rotation3d rot) {
return this.minus(other).rotateBy(rot).plus(other);
}
/**
* Returns a Translation2d representing this Translation3d projected into the X-Y plane.
*

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
View File

@@ -11,11 +11,15 @@ import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelPositionsProto;
import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelPositionsStruct;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Objects;
/** Represents the wheel positions for a differential drive drivetrain. */
public class DifferentialDriveWheelPositions
implements Interpolatable<DifferentialDriveWheelPositions> {
implements StructSerializable,
ProtobufSerializable,
Interpolatable<DifferentialDriveWheelPositions> {
/** Distance measured by the left side. */
public double leftMeters;

12
wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
View File

@@ -20,7 +20,7 @@ public final class LinearSystemId {
/**
* Create a state-space model of an elevator system. The states of the system are [position,
* velocity]ᵀ, inputs are [voltage], and outputs are [position].
* velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the carriage.
* @param massKg The mass of the elevator carriage, in kilograms.
@@ -88,8 +88,8 @@ public final class LinearSystemId {
/**
* Create a state-space model of a DC motor system. The states of the system are [angular
* position, angular velocity], inputs are [voltage], and outputs are [angular position, angular
* velocity].
* position, angular velocity]ᵀ, inputs are [voltage], and outputs are [angular position, angular
* velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to system.
* @param JKgMetersSquared The moment of inertia J of the DC motor.
@@ -125,7 +125,7 @@ public final class LinearSystemId {
/**
* Create a state-space model of a DC motor system from its kV (volts/(unit/sec)) and kA
* (volts/(unit/sec²)). These constants can be found using SysId. the states of the system are
* [position, velocity], inputs are [voltage], and outputs are [position].
* [position, velocity]ᵀ, inputs are [voltage], and outputs are [position].
*
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
* {@link edu.wpi.first.math.util.Units} class for converting between unit types.
@@ -211,7 +211,7 @@ public final class LinearSystemId {
/**
* Create a state-space model of a single jointed arm system. The states of the system are [angle,
* angular velocity], inputs are [voltage], and outputs are [angle].
* angular velocity]ᵀ, inputs are [voltage], and outputs are [angle, angular velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the arm.
* @param JKgSquaredMeters The moment of inertia J of the arm.
@@ -279,7 +279,7 @@ public final class LinearSystemId {
/**
* Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA
* (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
* [position, velocity]ᵀ, inputs are [voltage], and outputs are [position].
* [position, velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
*
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
* {@link edu.wpi.first.math.util.Units} class for converting between unit types.

12
wpimath/src/main/native/include/frc/ct_matrix.h
View File

@@ -163,8 +163,8 @@ class ct_matrix {
if (std::is_constant_evaluated()) {
ct_matrix<Scalar, Rows, Cols> result;
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 3; ++col) {
for (int row = 0; row < rhs.rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) {
result(row, col) = lhs(row, col) + rhs(row, col);
}
}
@@ -188,8 +188,8 @@ class ct_matrix {
if (std::is_constant_evaluated()) {
ct_matrix<Scalar, Rows, Cols> result;
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 3; ++col) {
for (int row = 0; row < rhs.rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) {
result(row, col) = lhs(row, col) - rhs(row, col);
}
}
@@ -282,8 +282,8 @@ class ct_matrix {
if (std::is_constant_evaluated()) {
Scalar sum = 0.0;
for (int row = 0; row < Rows; ++row) {
for (int col = 0; col < Cols; ++col) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
sum += (*this)(row, col) * (*this)(row, col);
}
}

13
wpimath/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -184,6 +184,19 @@ class WPILIB_DLLEXPORT Pose2d {
*/
constexpr Pose2d RelativeTo(const Pose2d& other) const;
/**
* Rotates the current pose around a point in 2D space.
*
* @param point The point in 2D space to rotate around.
* @param rot The rotation to rotate the pose by.
*
* @return The new rotated pose.
*/
constexpr Pose2d RotateAround(const Translation2d& point,
const Rotation2d& rot) const {
return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)};
}
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
*

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

@@ -207,6 +207,19 @@ class WPILIB_DLLEXPORT Pose3d {
*/
constexpr Pose3d RelativeTo(const Pose3d& other) const;
/**
* Rotates the current pose around a point in 3D space.
*
* @param point The point in 3D space to rotate around.
* @param rot The rotation to rotate the pose by.
*
* @return The new rotated pose.
*/
constexpr Pose3d RotateAround(const Translation3d& point,
const Rotation3d& rot) const {
return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)};
}
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*

4
wpimath/src/main/native/include/frc/geometry/Rotation2d.h
View File

@@ -49,11 +49,11 @@ class WPILIB_DLLEXPORT Rotation2d {
constexpr Rotation2d(double x, double y) {
double magnitude = gcem::hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
m_sin = y / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
m_sin = 0.0;
if (!std::is_constant_evaluated()) {
wpi::math::MathSharedStore::ReportError(
"x and y components of Rotation2d are zero\n{}",

12
wpimath/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -148,6 +148,18 @@ class WPILIB_DLLEXPORT Translation3d {
units::meter_t{qprime.Z()}};
}
/**
* Rotates this translation around another translation in 3D space.
*
* @param other The other translation to rotate around.
* @param rot The rotation to rotate the translation by.
* @return The new rotated translation.
*/
constexpr Translation3d RotateAround(const Translation3d& other,
const Rotation3d& rot) const {
return (*this - other).RotateBy(rot) + other;
}
/**
* Returns a Translation2d representing this Translation3d projected into the
* X-Y plane.

34
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
View File

@@ -37,7 +37,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Create a state-space model of the elevator system. The states of the system
* are [position, velocity], inputs are [voltage], and outputs are [position].
* are [position, velocity]ᵀ, inputs are [voltage], and outputs are [position,
* velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the carriage.
* @param mass The mass of the elevator carriage, in kilograms.
@@ -74,8 +75,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Create a state-space model of a single-jointed arm system.The states of the
* system are [angle, angular velocity], inputs are [voltage], and outputs are
* [angle].
* system are [angle, angular velocity]ᵀ, inputs are [voltage], and outputs
* are [angle, angular velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the arm.
* @param J The moment of inertia J of the arm.
@@ -147,8 +148,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Create a state-space model for a 1 DOF position system from its kV
* (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
* found using SysId. the states of the system are [position, velocity],
* inputs are [voltage], and outputs are [position].
* found using SysId. the states of the system are [position, velocity]ᵀ,
* inputs are [voltage], and outputs are [position, velocity]ᵀ.
*
* You MUST use an SI unit (i.e. meters or radians) for the Distance template
* argument. You may still use non-SI units (such as feet or inches) for the
@@ -169,7 +170,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
template <typename Distance>
requires std::same_as<units::meter, Distance> ||
std::same_as<units::radian, Distance>
static constexpr LinearSystem<2, 1, 1> IdentifyPositionSystem(
static constexpr LinearSystem<2, 1, 2> IdentifyPositionSystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
if (kV < decltype(kV){0}) {
@@ -180,11 +181,11 @@ class WPILIB_DLLEXPORT LinearSystemId {
}
Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{{0.0}, {0.0}};
return LinearSystem<2, 1, 1>(A, B, C, D);
return LinearSystem<2, 1, 2>(A, B, C, D);
}
/**
@@ -337,8 +338,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Create a state-space model of a DC motor system. The states of the system
* are [angular position, angular velocity], inputs are [voltage], and outputs
* are [angular position, angular velocity].
* are [angular position, angular velocity]ᵀ, inputs are [voltage], and
* outputs are [angular position, angular velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the system.
* @param J the moment of inertia J of the DC motor.
@@ -370,8 +371,9 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Create a state-space model of a DC motor system from its kV
* (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
* found using SysId. the states of the system are [position, velocity],
* inputs are [voltage], and outputs are [position].
* found using SysId. the states of the system are [angular position, angular
* velocity]ᵀ, inputs are [voltage], and outputs are [angular position,
* angular velocity]ᵀ.
*
* You MUST use an SI unit (i.e. meters or radians) for the Distance template
* argument. You may still use non-SI units (such as feet or inches) for the
@@ -410,9 +412,9 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Create a state-space model of differential drive drivetrain. In this model,
* the states are [left velocity, right velocity], the inputs are [left
* the states are [left velocity, right velocity]ᵀ, the inputs are [left
* voltage, right voltage], and the outputs are [left velocity, right
* velocity]
* velocity]ᵀ.
*
* @param motor The motor (or gearbox) driving the drivetrain.
* @param mass The mass of the robot in kilograms.

153
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h vendored
View File

@@ -263,7 +263,7 @@ struct copy_using_evaluator_innervec_CompleteUnrolling {
DstAlignment = Kernel::AssignmentTraits::DstAlignment
};
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) {
kernel.template assignPacketByOuterInner<DstAlignment, SrcAlignment, PacketType>(outer, inner);
enum { NextIndex = Index + unpacket_traits<PacketType>::size };
copy_using_evaluator_innervec_CompleteUnrolling<Kernel, NextIndex, Stop>::run(kernel);
@@ -431,17 +431,25 @@ struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, NoUnrolling> {
template <typename Kernel>
struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, CompleteUnrolling> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
typedef typename Kernel::PacketType PacketType;
if (internal::is_constant_evaluated()) {
for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
kernel.assignCoeffByOuterInner(outer, inner);
}
}
} else {
typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
typedef typename Kernel::PacketType PacketType;
enum {
size = DstXprType::SizeAtCompileTime,
packetSize = unpacket_traits<PacketType>::size,
alignedSize = (int(size) / packetSize) * packetSize
};
enum {
size = DstXprType::SizeAtCompileTime,
packetSize = unpacket_traits<PacketType>::size,
alignedSize = (int(size) / packetSize) * packetSize
};
copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
}
}
};
@@ -465,9 +473,17 @@ struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, NoUnrolling> {
template <typename Kernel>
struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, CompleteUnrolling> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) {
if (internal::is_constant_evaluated()) {
for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
kernel.assignCoeffByOuterInner(outer, inner);
}
}
} else {
typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
}
}
};
@@ -498,8 +514,16 @@ struct dense_assignment_loop<Kernel, LinearTraversal, NoUnrolling> {
template <typename Kernel>
struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
if (internal::is_constant_evaluated()) {
for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
kernel.assignCoeffByOuterInner(outer, inner);
}
}
} else {
typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
}
}
};
@@ -510,41 +534,49 @@ struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling> {
template <typename Kernel>
struct dense_assignment_loop<Kernel, SliceVectorizedTraversal, NoUnrolling> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
typedef typename Kernel::Scalar Scalar;
typedef typename Kernel::PacketType PacketType;
enum {
packetSize = unpacket_traits<PacketType>::size,
requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment),
alignable =
packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar),
dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment),
dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment)
};
const Scalar* dst_ptr = kernel.dstDataPtr();
if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) {
// the pointer is not aligned-on scalar, so alignment is not possible
return dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>::run(kernel);
}
const Index packetAlignedMask = packetSize - 1;
const Index innerSize = kernel.innerSize();
const Index outerSize = kernel.outerSize();
const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0;
Index alignedStart =
((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
if (internal::is_constant_evaluated()) {
for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
kernel.assignCoeffByOuterInner(outer, inner);
}
}
} else {
typedef typename Kernel::Scalar Scalar;
typedef typename Kernel::PacketType PacketType;
enum {
packetSize = unpacket_traits<PacketType>::size,
requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment),
alignable =
packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar),
dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment),
dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment)
};
const Scalar* dst_ptr = kernel.dstDataPtr();
if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) {
// the pointer is not aligned-on scalar, so alignment is not possible
return dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>::run(kernel);
}
const Index packetAlignedMask = packetSize - 1;
const Index innerSize = kernel.innerSize();
const Index outerSize = kernel.outerSize();
const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0;
Index alignedStart =
((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
for (Index outer = 0; outer < outerSize; ++outer) {
const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask);
// do the non-vectorizable part of the assignment
for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
for (Index outer = 0; outer < outerSize; ++outer) {
const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask);
// do the non-vectorizable part of the assignment
for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
// do the vectorizable part of the assignment
for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize)
kernel.template assignPacketByOuterInner<dstAlignment, Unaligned, PacketType>(outer, inner);
// do the vectorizable part of the assignment
for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize)
kernel.template assignPacketByOuterInner<dstAlignment, Unaligned, PacketType>(outer, inner);
// do the non-vectorizable part of the assignment
for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
// do the non-vectorizable part of the assignment
for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize);
alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize);
}
}
}
};
@@ -594,9 +626,9 @@ class generic_dense_assignment_kernel {
typedef copy_using_evaluator_traits<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor> AssignmentTraits;
typedef typename AssignmentTraits::PacketType PacketType;
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE generic_dense_assignment_kernel(DstEvaluatorType& dst,
const SrcEvaluatorType& src,
const Functor& func, DstXprType& dstExpr)
EIGEN_DEVICE_FUNC
EIGEN_STRONG_INLINE constexpr generic_dense_assignment_kernel(DstEvaluatorType& dst, const SrcEvaluatorType& src,
const Functor& func, DstXprType& dstExpr)
: m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr) {
#ifdef EIGEN_DEBUG_ASSIGN
AssignmentTraits::debug();
@@ -614,7 +646,7 @@ class generic_dense_assignment_kernel {
EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const EIGEN_NOEXCEPT { return m_src; }
/// Assign src(row,col) to dst(row,col) through the assignment functor.
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col) {
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(Index row, Index col) {
m_functor.assignCoeff(m_dst.coeffRef(row, col), m_src.coeff(row, col));
}
@@ -624,7 +656,7 @@ class generic_dense_assignment_kernel {
}
/// \sa assignCoeff(Index,Index)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner) {
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeffByOuterInner(Index outer, Index inner) {
Index row = rowIndexByOuterInner(outer, inner);
Index col = colIndexByOuterInner(outer, inner);
assignCoeff(row, col);
@@ -648,7 +680,7 @@ class generic_dense_assignment_kernel {
assignPacket<StoreMode, LoadMode, Packet>(row, col);
}
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index rowIndexByOuterInner(Index outer, Index inner) {
typedef typename DstEvaluatorType::ExpressionTraits Traits;
return int(Traits::RowsAtCompileTime) == 1 ? 0
: int(Traits::ColsAtCompileTime) == 1 ? inner
@@ -656,7 +688,7 @@ class generic_dense_assignment_kernel {
: inner;
}
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index colIndexByOuterInner(Index outer, Index inner) {
typedef typename DstEvaluatorType::ExpressionTraits Traits;
return int(Traits::ColsAtCompileTime) == 1 ? 0
: int(Traits::RowsAtCompileTime) == 1 ? inner
@@ -708,8 +740,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, co
}
template <typename DstXprType, typename SrcXprType, typename T1, typename T2>
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src,
const internal::assign_op<T1, T2>& /*func*/) {
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize_if_allowed(DstXprType& dst, const SrcXprType& src,
const internal::assign_op<T1, T2>& /*func*/) {
Index dstRows = src.rows();
Index dstCols = src.cols();
if (((dst.rows() != dstRows) || (dst.cols() != dstCols))) dst.resize(dstRows, dstCols);
@@ -790,7 +822,7 @@ struct Assignment;
// not has to bother about these annoying details.
template <typename Dst, typename Src>
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(Dst& dst, const Src& src) {
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(Dst& dst, const Src& src) {
call_assignment(dst, src, internal::assign_op<typename Dst::Scalar, typename Src::Scalar>());
}
template <typename Dst, typename Src>
@@ -807,7 +839,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment(
}
template <typename Dst, typename Src, typename Func>
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(
Dst& dst, const Src& src, const Func& func, std::enable_if_t<!evaluator_assume_aliasing<Src>::value, void*> = 0) {
call_assignment_no_alias(dst, src, func);
}
@@ -891,9 +923,12 @@ EIGEN_DEVICE_FUNC void check_for_aliasing(const Dst& dst, const Src& src);
// both partial specialization+SFINAE without ambiguous specialization
template <typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
struct Assignment<DstXprType, SrcXprType, Functor, Dense2Dense, Weak> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, const Functor& func) {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(DstXprType& dst, const SrcXprType& src,
const Functor& func) {
#ifndef EIGEN_NO_DEBUG
internal::check_for_aliasing(dst, src);
if (!internal::is_constant_evaluated()) {
internal::check_for_aliasing(dst, src);
}
#endif
call_dense_assignment_loop(dst, src, func);

2
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h vendored
View File

@@ -50,7 +50,7 @@ struct EigenBase {
/** \returns a const reference to the derived object */
EIGEN_DEVICE_FUNC constexpr const Derived& derived() const { return *static_cast<const Derived*>(this); }
EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const {
EIGEN_DEVICE_FUNC inline constexpr Derived& const_cast_derived() const {
return *static_cast<Derived*>(const_cast<EigenBase*>(this));
}
EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast<const Derived*>(this); }

2
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h vendored
View File

@@ -23,7 +23,7 @@ namespace internal {
*/
template <typename DstScalar, typename SrcScalar>
struct assign_op {
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; }
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; }
template <int Alignment, typename Packet>
EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const {

13
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
View File

@@ -72,6 +72,19 @@ class Pose2dTest {
() -> assertEquals(0.0, finalRelativeToInitial.getRotation().getDegrees(), kEpsilon));
}
@Test
void testRotateAround() {
var initial = new Pose2d(5, 0, Rotation2d.kZero);
var point = Translation2d.kZero;
var rotated = initial.rotateAround(point, Rotation2d.kPi);
assertAll(
() -> assertEquals(-5.0, rotated.getX(), kEpsilon),
() -> assertEquals(0.0, rotated.getY(), kEpsilon),
() -> assertEquals(180.0, rotated.getRotation().getDegrees(), kEpsilon));
}
@Test
void testEquality() {
var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));

13
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
View File

@@ -136,6 +136,19 @@ class Pose3dTest {
() -> assertEquals(0.0, finalRelativeToInitial.getRotation().getZ(), kEpsilon));
}
@Test
void testRotateAround() {
var initial = new Pose3d(new Translation3d(5, 0, 0), Rotation3d.kZero);
var point = Translation3d.kZero;
var rotated = initial.rotateAround(point, new Rotation3d(0, 0, Math.PI));
assertAll(
() -> assertEquals(-5.0, rotated.getX(), kEpsilon),
() -> assertEquals(0.0, rotated.getY(), kEpsilon),
() -> assertEquals(Math.PI, rotated.getRotation().getZ(), kEpsilon));
}
@Test
void testEquality() {
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);

34
wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
View File

@@ -78,6 +78,40 @@ class Translation3dTest {
() -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
}
@Test
void testRotateAround() {
var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
var translation = new Translation3d(1.0, 2.0, 3.0);
var around = new Translation3d(3.0, 2.0, 1.0);
var rotated1 =
translation.rotateAround(around, new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
assertAll(
() -> assertEquals(1.0, rotated1.getX(), kEpsilon),
() -> assertEquals(0.0, rotated1.getY(), kEpsilon),
() -> assertEquals(1.0, rotated1.getZ(), kEpsilon));
var rotated2 =
translation.rotateAround(around, new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
assertAll(
() -> assertEquals(5.0, rotated2.getX(), kEpsilon),
() -> assertEquals(2.0, rotated2.getY(), kEpsilon),
() -> assertEquals(3.0, rotated2.getZ(), kEpsilon));
var rotated3 =
translation.rotateAround(around, new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
assertAll(
() -> assertEquals(3.0, rotated3.getX(), kEpsilon),
() -> assertEquals(0.0, rotated3.getY(), kEpsilon),
() -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
}
@Test
void testToTranslation2d() {
var translation = new Translation3d(1.0, 2.0, 3.0);

11
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -51,6 +51,17 @@ TEST(Pose2dTest, RelativeTo) {
EXPECT_NEAR(0.0, finalRelativeToInitial.Rotation().Degrees().value(), 1e-9);
}
TEST(Pose2dTest, RotateAround) {
const Pose2d initial{5_m, 0_m, 0_deg};
const Translation2d point{0_m, 0_m};
const auto rotated = initial.RotateAround(point, Rotation2d{180_deg});
EXPECT_NEAR(-5.0, rotated.X().value(), 1e-9);
EXPECT_NEAR(0.0, rotated.Y().value(), 1e-9);
EXPECT_NEAR(180.0, rotated.Rotation().Degrees().value(), 1e-9);
}
TEST(Pose2dTest, Equality) {
const Pose2d a{0_m, 5_m, 43_deg};
const Pose2d b{0_m, 5_m, 43_deg};

13
wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
View File

@@ -83,6 +83,19 @@ TEST(Pose3dTest, RelativeTo) {
EXPECT_NEAR(0.0, finalRelativeToInitial.Rotation().Z().value(), 1e-9);
}
TEST(Pose3dTest, RotateAround) {
const Pose3d initial{5_m, 0_m, 0_m, Rotation3d{}};
const Translation3d point{0_m, 0_m, 0_m};
const auto rotated =
initial.RotateAround(point, Rotation3d{0_deg, 0_deg, 180_deg});
EXPECT_NEAR(-5.0, rotated.X().value(), 1e-9);
EXPECT_NEAR(0.0, rotated.Y().value(), 1e-9);
EXPECT_NEAR(units::radian_t{180_deg}.value(), rotated.Rotation().Z().value(),
1e-9);
}
TEST(Pose3dTest, Equality) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};

5
wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
View File

@@ -79,8 +79,13 @@ TEST(Rotation2dTest, Inequality) {
}
TEST(Rotation2dTest, ToMatrix) {
#if __GNUC__ <= 11
Rotation2d before{20_deg};
Rotation2d after{before.ToMatrix()};
#else
constexpr Rotation2d before{20_deg};
constexpr Rotation2d after{before.ToMatrix()};
#endif
EXPECT_EQ(before, after);
}

5
wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
View File

@@ -308,8 +308,13 @@ TEST(Rotation3dTest, Inequality) {
}
TEST(Rotation3dTest, ToMatrix) {
#if __GNUC__ <= 11
Rotation3d before{10_deg, 20_deg, 30_deg};
Rotation3d after{before.ToMatrix()};
#else
constexpr Rotation3d before{10_deg, 20_deg, 30_deg};
constexpr Rotation3d after{before.ToMatrix()};
#endif
EXPECT_EQ(before, after);
}

11
wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
View File

@@ -35,7 +35,16 @@ TEST(Translation2dTest, RotateBy) {
const auto rotated = another.RotateBy(90_deg);
EXPECT_NEAR(0.0, rotated.X().value(), 1e-9);
EXPECT_DOUBLE_EQ(3.0, rotated.Y().value());
EXPECT_NEAR(3.0, rotated.Y().value(), 1e-9);
}
TEST(Translation2dTest, RotateAround) {
const Translation2d translation{2_m, 1_m};
const Translation2d other{3_m, 2_m};
const auto rotated = translation.RotateAround(other, 180_deg);
EXPECT_NEAR(4.0, rotated.X().value(), 1e-9);
EXPECT_NEAR(3.0, rotated.Y().value(), 1e-9);
}
TEST(Translation2dTest, Multiplication) {

27
wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
View File

@@ -57,6 +57,33 @@ TEST(Translation3dTest, RotateBy) {
EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
}
TEST(Translation3dTest, RotateAround) {
Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Translation3d translation{1_m, 2_m, 3_m};
const Translation3d around{3_m, 2_m, 1_m};
const auto rotated1 =
translation.RotateAround(around, Rotation3d{xAxis, 90_deg});
EXPECT_NEAR(rotated1.X().value(), 1.0, kEpsilon);
EXPECT_NEAR(rotated1.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(rotated1.Z().value(), 1.0, kEpsilon);
const auto rotated2 =
translation.RotateAround(around, Rotation3d{yAxis, 90_deg});
EXPECT_NEAR(rotated2.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(rotated2.Y().value(), 2.0, kEpsilon);
EXPECT_NEAR(rotated2.Z().value(), 3.0, kEpsilon);
const auto rotated3 =
translation.RotateAround(around, Rotation3d{zAxis, 90_deg});
EXPECT_NEAR(rotated3.X().value(), 3.0, kEpsilon);
EXPECT_NEAR(rotated3.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
}
TEST(Translation3dTest, ToTranslation2d) {
Translation3d translation{1_m, 2_m, 3_m};
Translation2d expected{1_m, 2_m};

39
wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
View File

@@ -12,8 +12,13 @@
#include "units/mass.h"
TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::DrivetrainVelocitySystem(
frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
#else
constexpr auto model = frc::LinearSystemId::DrivetrainVelocitySystem(
frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
#endif
ASSERT_TRUE(model.A().isApprox(
frc::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}}, 0.001));
@@ -37,8 +42,14 @@ TEST(LinearSystemIDTest, ElevatorSystem) {
}
TEST(LinearSystemIDTest, FlywheelSystem) {
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
#else
constexpr auto model = frc::LinearSystemId::FlywheelSystem(
frc::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#endif
ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-26.87032}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1354.166667}, 0.001));
ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 1>{1.0}, 0.001));
@@ -46,8 +57,14 @@ TEST(LinearSystemIDTest, FlywheelSystem) {
}
TEST(LinearSystemIDTest, DCMotorSystem) {
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::DCMotorSystem(frc::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
#else
constexpr auto model = frc::LinearSystemId::DCMotorSystem(
frc::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#endif
ASSERT_TRUE(
model.A().isApprox(frc::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0, 1354.166667}, 0.001));
@@ -59,10 +76,17 @@ TEST(LinearSystemIDTest, DCMotorSystem) {
TEST(LinearSystemIDTest, IdentifyPositionSystem) {
// By controls engineering in frc,
// x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
double kv = 1.0;
double ka = 0.5;
constexpr double kv = 1.0;
constexpr double ka = 0.5;
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#else
constexpr auto model =
frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#endif
ASSERT_TRUE(model.A().isApprox(
frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
@@ -73,10 +97,17 @@ TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
// By controls engineering in frc,
// V = kv * velocity + ka * acceleration
// x-dot = -kv/ka * v + 1/ka \cdot V
double kv = 1.0;
double ka = 0.5;
constexpr double kv = 1.0;
constexpr double ka = 0.5;
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#else
constexpr auto model =
frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#endif
ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-kv / ka}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1.0 / ka}, 0.001));