From 0ca1e9b5f9fa3739f3ec1dcff896f369bd431c94 Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Fri, 17 Nov 2023 11:45:04 -0500 Subject: [PATCH] [wpimath] Add basic wpiunits support (#5821) To reduce the need for users to manually perform unit conversions, this allows Measure objects from wpiunits to be passed into most places in wpimath that currently expect doubles in terms of SI units like meters. For example, users would need to know that unit conversion is required - and what the correct units are. Using units would be more difficult to write code for than just hardcoding a value or using Units.inchesToMeters. Now, using units has no more developer overhead than using raw numbers. --- CMakeLists.txt | 18 ++++ README-CMAKE.md | 3 + apriltag/CMakeLists.txt | 2 +- romiVendordep/CMakeLists.txt | 2 +- wpilibNewCommands/CMakeLists.txt | 2 +- wpilibj/CMakeLists.txt | 2 +- wpimath/CMakeLists.txt | 2 +- wpimath/build.gradle | 1 + .../edu/wpi/first/math/geometry/Pose2d.java | 16 ++++ .../wpi/first/math/geometry/Rotation2d.java | 13 +++ .../wpi/first/math/geometry/Transform2d.java | 16 ++++ .../first/math/geometry/Translation2d.java | 15 +++ .../first/math/geometry/Translation3d.java | 16 ++++ .../first/math/kinematics/ChassisSpeeds.java | 95 +++++++++++++++++++ .../DifferentialDriveKinematics.java | 15 +++ .../kinematics/DifferentialDriveOdometry.java | 51 ++++++++++ .../DifferentialDriveWheelPositions.java | 14 +++ .../DifferentialDriveWheelSpeeds.java | 31 ++++++ .../MecanumDriveWheelPositions.java | 20 ++++ .../kinematics/MecanumDriveWheelSpeeds.java | 39 ++++++++ .../kinematics/SwerveDriveKinematics.java | 56 +++++++++++ .../math/kinematics/SwerveModulePosition.java | 14 +++ .../math/kinematics/SwerveModuleState.java | 15 +++ .../math/trajectory/TrajectoryConfig.java | 38 ++++++++ .../math/trajectory/TrapezoidProfile.java | 12 +++ .../wpi/first/math/geometry/Pose2dTest.java | 10 ++ .../first/math/geometry/Rotation2dTest.java | 8 ++ .../first/math/geometry/Transform2dTest.java | 13 +++ .../math/geometry/Translation3dTest.java | 10 ++ .../math/kinematics/ChassisSpeedsTest.java | 15 +++ wpimath/wpimath-config.cmake.in | 1 + wpiunits/CMakeLists.txt | 25 +++++ .../java/edu/wpi/first/units/BaseUnits.java | 6 +- .../main/java/edu/wpi/first/units/Units.java | 7 +- .../java/edu/wpi/first/units/UnitsTest.java | 3 +- wpiunits/wpiunits-config.cmake | 2 + xrpVendordep/CMakeLists.txt | 2 +- 37 files changed, 596 insertions(+), 14 deletions(-) create mode 100644 wpiunits/CMakeLists.txt create mode 100644 wpiunits/wpiunits-config.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index f994bf13e5..3929280c99 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,6 +60,7 @@ option(WITH_JAVA_SOURCE "Build Java source jars" ON) option(WITH_CSCORE "Build cscore (needs OpenCV)" ON) option(WITH_NTCORE "Build ntcore" ON) option(WITH_WPIMATH "Build wpimath" ON) +option(WITH_WPIUNITS "Build wpiunits" ON) option(WITH_WPILIB "Build hal, wpilibc/j, and myRobot (needs OpenCV)" ON) option(WITH_EXAMPLES "Build examples" OFF) option(WITH_TESTS "Build unit tests (requires internet connection)" ON) @@ -154,6 +155,13 @@ FATAL: Cannot build wpilib without wpimath. ") endif() +if (NOT WITH_WPIUNITS AND WITH_WPIMATH AND WITH_JAVA) + message(FATAL_ERROR " +FATAL: Cannot build Java wpimath without wpiunits. + Enable wpiunits by setting WITH_WPIUNITS=ON or disable the Java build by setting WITH_JAVA=OFF +") +endif() + set( wpilib_dest "") set( include_dest include ) set( java_lib_dest java ) @@ -191,6 +199,7 @@ set(CSCORE_DEP_REPLACE_IMPL "include(\${SELF_DIR}/cscore-config.cmake)") set(CAMERASERVER_DEP_REPLACE_IMPL "include(\${SELF_DIR}/cameraserver-config.cmake)") set(HAL_DEP_REPLACE_IMPL "include(\${SELF_DIR}/hal-config.cmake)") set(WPIMATH_DEP_REPLACE "include($\{SELF_DIR\}/wpimath-config.cmake)") +set(WPIUNITS_DEP_REPLACE "include($\{SELF_DIR\}/wpiunits-config.cmake)") set(WPILIBC_DEP_REPLACE_IMPL "include(\${SELF_DIR}/wpilibc-config.cmake)") set(WPILIBNEWCOMMANDS_DEP_REPLACE "include(\${SELF_DIR}/wpilibNewcommands-config.cmake)") else() @@ -201,6 +210,7 @@ set(CSCORE_DEP_REPLACE_IMPL "find_dependency(cscore)") set(CAMERASERVER_DEP_REPLACE_IMPL "find_dependency(cameraserver)") set(HAL_DEP_REPLACE_IMPL "find_dependency(hal)") set(WPIMATH_DEP_REPLACE "find_dependency(wpimath)") +set(WPIUNITS_DEP_REPLACE "find_dependency(wpiunits)") set(WPILIBC_DEP_REPLACE_IMPL "find_dependency(wpilibc)") set(WPILIBNEWCOMMANDS_DEP_REPLACE "find_dependency(wpilibNewCommands)") endif() @@ -291,9 +301,17 @@ if (WITH_NTCORE) endif() if (WITH_WPIMATH) + if (WITH_JAVA) + add_subdirectory(wpiunits) + endif() add_subdirectory(wpimath) endif() +if (WITH_WPIUNITS AND NOT WITH_WPIMATH) + # In case of building wpiunits standalone + add_subdirectory(wpiunits) +endif() + if (WITH_GUI) add_subdirectory(fieldImages) add_subdirectory(imgui) diff --git a/README-CMAKE.md b/README-CMAKE.md index 776f808d91..a2750bd70d 100644 --- a/README-CMAKE.md +++ b/README-CMAKE.md @@ -12,6 +12,7 @@ WPILib is normally built with Gradle, however for some systems, such as Linux ba * halsim * wpigui * wpimath +* wpiunits * wpilibNewCommands By default, all libraries except for the HAL and WPILib get built with a default CMake setup. The libraries are built as shared libraries, and include the JNI libraries as well as building the Java JARs. @@ -44,6 +45,8 @@ The following build options are available: * This option will cause ntcore to be built. Turning this off will implicitly disable wpinet and wpilib as well, irrespective of their specific options. * `WITH_WPIMATH` (ON Default) * This option will build the wpimath library. This option must be on to build wpilib. +* `WITH_WPIUNITS` (ON Default) + * This option will build the wpiunits library. This option must be on to build the Java wpimath library and requires `WITH_JAVA` to also be on. * `WITH_WPILIB` (ON Default) * This option will build the hal and wpilibc/j during the build. The HAL is the simulator hal, unless the external hal options are used. The cmake build has no capability to build for the RoboRIO. * `WITH_EXAMPLES` (ON Default) diff --git a/apriltag/CMakeLists.txt b/apriltag/CMakeLists.txt index 9d97363760..e86c47f5ed 100644 --- a/apriltag/CMakeLists.txt +++ b/apriltag/CMakeLists.txt @@ -44,7 +44,7 @@ if (WITH_JAVA) add_jar(apriltag_jar SOURCES ${JAVA_SOURCES} RESOURCES NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES} - INCLUDE_JARS wpimath_jar ${EJML_JARS} wpiutil_jar ${OPENCV_JAR_FILE} + INCLUDE_JARS wpimath_jar wpiunits_jar ${EJML_JARS} wpiutil_jar ${OPENCV_JAR_FILE} OUTPUT_NAME apriltag GENERATE_NATIVE_HEADERS apriltag_jni_headers) diff --git a/romiVendordep/CMakeLists.txt b/romiVendordep/CMakeLists.txt index bd5f413fc5..72b55c815b 100644 --- a/romiVendordep/CMakeLists.txt +++ b/romiVendordep/CMakeLists.txt @@ -10,7 +10,7 @@ if (WITH_JAVA) set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) - add_jar(romiVendordep_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME romiVendordep) + add_jar(romiVendordep_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiunits_jar wpiutil_jar wpilibj_jar OUTPUT_NAME romiVendordep) get_property(ROMIVENDORDEP_JAR_FILE TARGET romiVendordep_jar PROPERTY JAR_FILE) install(FILES ${ROMIVENDORDEP_JAR_FILE} DESTINATION "${java_lib_dest}") diff --git a/wpilibNewCommands/CMakeLists.txt b/wpilibNewCommands/CMakeLists.txt index 34a6bb1f5f..299b92a861 100644 --- a/wpilibNewCommands/CMakeLists.txt +++ b/wpilibNewCommands/CMakeLists.txt @@ -10,7 +10,7 @@ if (WITH_JAVA) set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) - add_jar(wpilibNewCommands_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME wpilibNewCommands) + add_jar(wpilibNewCommands_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiunits_jar wpiutil_jar wpilibj_jar OUTPUT_NAME wpilibNewCommands) get_property(WPILIBNEWCOMMANDS_JAR_FILE TARGET wpilibNewCommands_jar PROPERTY JAR_FILE) install(FILES ${WPILIBNEWCOMMANDS_JAR_FILE} DESTINATION "${java_lib_dest}") diff --git a/wpilibj/CMakeLists.txt b/wpilibj/CMakeLists.txt index 5924f4ddee..c2cea44d9a 100644 --- a/wpilibj/CMakeLists.txt +++ b/wpilibj/CMakeLists.txt @@ -17,7 +17,7 @@ if (WITH_JAVA) file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar") file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar") - add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${JACKSON_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpimath_jar wpiutil_jar OUTPUT_NAME wpilibj) + add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${JACKSON_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpimath_jar wpiunits_jar wpiutil_jar OUTPUT_NAME wpilibj) get_property(WPILIBJ_JAR_FILE TARGET wpilibj_jar PROPERTY JAR_FILE) install(FILES ${WPILIBJ_JAR_FILE} DESTINATION "${java_lib_dest}") diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt index 997ec46e9f..bb3f99c37e 100644 --- a/wpimath/CMakeLists.txt +++ b/wpimath/CMakeLists.txt @@ -110,7 +110,7 @@ if (WITH_JAVA) file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${WPILIB_BINARY_DIR}/wpimath/generated/*.java) - add_jar(wpimath_jar ${JAVA_SOURCES} ${WPIMATH_QUICKBUF_SRCS} INCLUDE_JARS ${EJML_JARS} wpiutil_jar OUTPUT_NAME wpimath GENERATE_NATIVE_HEADERS wpimath_jni_headers) + add_jar(wpimath_jar ${JAVA_SOURCES} ${WPIMATH_QUICKBUF_SRCS} INCLUDE_JARS ${EJML_JARS} wpiutil_jar wpiunits_jar OUTPUT_NAME wpimath GENERATE_NATIVE_HEADERS wpimath_jni_headers) get_property(WPIMATH_JAR_FILE TARGET wpimath_jar PROPERTY JAR_FILE) install(FILES ${WPIMATH_JAR_FILE} DESTINATION "${java_lib_dest}") diff --git a/wpimath/build.gradle b/wpimath/build.gradle index 3496ba7f98..32ea0a1b56 100644 --- a/wpimath/build.gradle +++ b/wpimath/build.gradle @@ -43,6 +43,7 @@ model { } dependencies { + api project(":wpiunits") api "org.ejml:ejml-simple:0.43.1" api "com.fasterxml.jackson.core:jackson-annotations:2.15.2" api "com.fasterxml.jackson.core:jackson-core:2.15.2" diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index bc7f536c25..032522b864 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -4,12 +4,16 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Meters; + 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 edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; import java.nio.ByteBuffer; @@ -58,6 +62,18 @@ public class Pose2d implements Interpolatable { m_rotation = rotation; } + /** + * Constructs a pose with x and y translations instead of a separate Translation2d. The X and Y + * translations will be converted to and tracked as meters. + * + * @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(Measure x, Measure y, Rotation2d rotation) { + this(x.in(Meters), y.in(Meters), rotation); + } + /** * Transforms the pose by the given transformation and returns the new transformed pose. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 0556669d7d..55f42e601d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Radians; + import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; @@ -12,6 +14,8 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; import java.nio.ByteBuffer; @@ -69,6 +73,15 @@ public class Rotation2d implements Interpolatable { m_value = Math.atan2(m_sin, m_cos); } + /** + * Constructs a Rotation2d with the given angle. + * + * @param angle The angle of the rotation. + */ + public Rotation2d(Measure angle) { + this(angle.in(Radians)); + } + /** * Constructs and returns a Rotation2d with the given radian value. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java index c7959ba953..ea39127b5c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java @@ -4,7 +4,11 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; import java.nio.ByteBuffer; @@ -57,6 +61,18 @@ public class Transform2d { m_rotation = rotation; } + /** + * Constructs a transform with x and y translations instead of a separate Translation2d. The X and + * Y translations will be converted to and tracked as meters. + * + * @param x The x component of the translational component of the transform. + * @param y The y component of the translational component of the transform. + * @param rotation The rotational component of the transform. + */ + public Transform2d(Measure x, Measure y, Rotation2d rotation) { + this(x.in(Meters), y.in(Meters), rotation); + } + /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform2d() { m_translation = new Translation2d(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index 96e0001f50..ebfce32045 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Meters; + import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; @@ -11,6 +13,8 @@ import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; import java.nio.ByteBuffer; @@ -63,6 +67,17 @@ public class Translation2d implements Interpolatable { m_y = distance * angle.getSin(); } + /** + * Constructs a Translation2d with the X and Y components equal to the provided values. The X and + * Y components will be converted to and tracked as meters. + * + * @param x The x component of the translation. + * @param y The y component of the translation. + */ + public Translation2d(Measure x, Measure y) { + this(x.in(Meters), y.in(Meters)); + } + /** * Calculates the distance between two translations in 2D space. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index bc55f650c8..7b690dfedf 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Meters; + import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; @@ -11,6 +13,8 @@ import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import edu.wpi.first.util.protobuf.Protobuf; import edu.wpi.first.util.struct.Struct; import java.nio.ByteBuffer; @@ -67,6 +71,18 @@ public class Translation3d implements Interpolatable { m_z = rectangular.getZ(); } + /** + * Constructs a Translation3d with the X, Y, and Z components equal to the provided values. The + * components will be converted to and tracked as meters. + * + * @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(Measure x, Measure y, Measure z) { + this(x.in(Meters), y.in(Meters), z.in(Meters)); + } + /** * Calculates the distance between two translations in 3D space. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index ffbce71cbf..43b67405d3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -4,9 +4,18 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Time; +import edu.wpi.first.units.Velocity; /** * Represents the speed of a robot chassis. Although this class contains similar members compared to @@ -44,6 +53,20 @@ public class ChassisSpeeds { this.omegaRadiansPerSecond = omegaRadiansPerSecond; } + /** + * Constructs a ChassisSpeeds object. + * + * @param vx Forward velocity. + * @param vy Sideways velocity. + * @param omega Angular velocity. + */ + public ChassisSpeeds( + Measure> vx, + Measure> vy, + Measure> omega) { + this(vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond)); + } + /** * Discretizes a continuous-time chassis speed. * @@ -86,6 +109,32 @@ public class ChassisSpeeds { *

This is useful for compensating for translational skew when translating and rotating a * swerve drivetrain. * + * @param vx Forward velocity. + * @param vy Sideways velocity. + * @param omega Angular velocity. + * @param dt The duration of the timestep the speeds should be applied for. + * @return Discretized ChassisSpeeds. + */ + public static ChassisSpeeds discretize( + Measure> vx, + Measure> vy, + Measure> omega, + Measure

This function converts a continous-time chassis speed into a discrete-time one such that + * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the + * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt + * along the y-axis, and omega * dt around the z-axis). + * + *

This is useful for compensating for translational skew when translating and rotating a + * swerve drivetrain. + * * @param continuousSpeeds The continuous speeds. * @param dtSeconds The duration of the timestep the speeds should be applied for. * @return Discretized ChassisSpeeds. @@ -123,6 +172,29 @@ public class ChassisSpeeds { return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond); } + /** + * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds + * object. + * + * @param vx The component of speed in the x direction relative to the field. Positive x is away + * from your alliance wall. + * @param vy The component of speed in the y direction relative to the field. Positive y is to + * your left when standing behind the alliance wall. + * @param omega The angular rate of the robot. + * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is + * considered to be zero when it is facing directly away from your alliance station wall. + * Remember that this should be CCW positive. + * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. + */ + public static ChassisSpeeds fromFieldRelativeSpeeds( + Measure> vx, + Measure> vy, + Measure> omega, + Rotation2d robotAngle) { + return fromFieldRelativeSpeeds( + vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle); + } + /** * Converts a user provided field-relative ChassisSpeeds object into a robot-relative * ChassisSpeeds object. @@ -168,6 +240,29 @@ public class ChassisSpeeds { return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond); } + /** + * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds + * object. + * + * @param vx The component of speed in the x direction relative to the robot. Positive x is + * towards the robot's front. + * @param vy The component of speed in the y direction relative to the robot. Positive y is + * towards the robot's left. + * @param omega The angular rate of the robot. + * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is + * considered to be zero when it is facing directly away from your alliance station wall. + * Remember that this should be CCW positive. + * @return ChassisSpeeds object representing the speeds in the field's frame of reference. + */ + public static ChassisSpeeds fromRobotRelativeSpeeds( + Measure> vx, + Measure> vy, + Measure> omega, + Rotation2d robotAngle) { + return fromRobotRelativeSpeeds( + vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle); + } + /** * Converts a user provided robot-relative ChassisSpeeds object into a field-relative * ChassisSpeeds object. diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java index a1fb2db371..87907249e7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java @@ -4,9 +4,13 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; /** * Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel @@ -32,6 +36,17 @@ public class DifferentialDriveKinematics MathSharedStore.reportUsage(MathUsageId.kKinematics_DifferentialDrive, 1); } + /** + * Constructs a differential drive kinematics object. + * + * @param trackWidth The track width of the drivetrain. Theoretically, this is the distance + * between the left wheels and right wheels. However, the empirical value may be larger than + * the physical measured value due to scrubbing effects. + */ + public DifferentialDriveKinematics(Measure trackWidth) { + this(trackWidth.in(Meters)); + } + /** * Returns a chassis speed from left and right component velocities using forward kinematics. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java index db5e8a3387..87d7bbf77f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java @@ -4,10 +4,14 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; /** * Class for differential drive odometry. Odometry allows you to track the robot's position on the @@ -41,6 +45,22 @@ public class DifferentialDriveOdometry extends Odometry leftDistance, + Measure rightDistance, + Pose2d initialPoseMeters) { + this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters); + } + /** * Constructs a DifferentialDriveOdometry object. * @@ -53,6 +73,18 @@ public class DifferentialDriveOdometry extends Odometry leftDistance, Measure rightDistance) { + this(gyroAngle, leftDistance, rightDistance, new Pose2d()); + } + /** * Resets the robot's position on the field. * @@ -75,6 +107,25 @@ public class DifferentialDriveOdometry extends OdometryThe gyroscope angle does not need to be reset here on the user's robot code. The library + * automatically takes care of offsetting the gyro angle. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition( + Rotation2d gyroAngle, + Measure leftDistance, + Measure rightDistance, + Pose2d poseMeters) { + resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters); + } + /** * Updates the robot position on the field using distance measurements from encoders. This method * is more numerically accurate than using velocities to integrate the pose and is also diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java index 18866afe97..c0a211e049 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java @@ -4,7 +4,11 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import java.util.Objects; public class DifferentialDriveWheelPositions @@ -26,6 +30,16 @@ public class DifferentialDriveWheelPositions this.rightMeters = rightMeters; } + /** + * Constructs a DifferentialDriveWheelPositions. + * + * @param left Distance measured by the left side. + * @param right Distance measured by the right side. + */ + public DifferentialDriveWheelPositions(Measure left, Measure right) { + this(left.in(Meters), right.in(Meters)); + } + @Override public boolean equals(Object obj) { if (obj instanceof DifferentialDriveWheelPositions) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java index ec874b65f3..72578226bb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java @@ -4,6 +4,12 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; + /** Represents the wheel speeds for a differential drive drivetrain. */ public class DifferentialDriveWheelSpeeds { /** Speed of the left side of the robot. */ @@ -26,6 +32,17 @@ public class DifferentialDriveWheelSpeeds { this.rightMetersPerSecond = rightMetersPerSecond; } + /** + * Constructs a DifferentialDriveWheelSpeeds. + * + * @param left The left speed. + * @param right The right speed. + */ + public DifferentialDriveWheelSpeeds( + Measure> left, Measure> right) { + this(left.in(MetersPerSecond), right.in(MetersPerSecond)); + } + /** * Renormalizes the wheel speeds if any either side is above the specified maximum. * @@ -46,6 +63,20 @@ public class DifferentialDriveWheelSpeeds { } } + /** + * Renormalizes the wheel speeds if any either side is above the specified maximum. + * + *

Sometimes, after inverse kinematics, the requested speed from one or more wheels may be + * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can + * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the + * absolute threshold, while maintaining the ratio of speeds between wheels. + * + * @param attainableMaxSpeed The absolute max speed that a wheel can reach. + */ + public void desaturate(Measure> attainableMaxSpeed) { + desaturate(attainableMaxSpeed.in(MetersPerSecond)); + } + /** * Adds two DifferentialDriveWheelSpeeds and returns the sum. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java index 2b284cb27b..2ebb77a134 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java @@ -4,7 +4,11 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import java.util.Objects; public class MecanumDriveWheelPositions implements WheelPositions { @@ -42,6 +46,22 @@ public class MecanumDriveWheelPositions implements WheelPositions frontLeft, + Measure frontRight, + Measure rearLeft, + Measure rearRight) { + this(frontLeft.in(Meters), frontRight.in(Meters), rearLeft.in(Meters), rearRight.in(Meters)); + } + @Override public boolean equals(Object obj) { if (obj instanceof MecanumDriveWheelPositions) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java index 63cef1838f..7bd2295e55 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java @@ -4,6 +4,11 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import java.util.stream.DoubleStream; public class MecanumDriveWheelSpeeds { @@ -41,6 +46,26 @@ public class MecanumDriveWheelSpeeds { this.rearRightMetersPerSecond = rearRightMetersPerSecond; } + /** + * Constructs a MecanumDriveWheelSpeeds. + * + * @param frontLeft Speed of the front left wheel. + * @param frontRight Speed of the front right wheel. + * @param rearLeft Speed of the rear left wheel. + * @param rearRight Speed of the rear right wheel. + */ + public MecanumDriveWheelSpeeds( + Measure> frontLeft, + Measure> frontRight, + Measure> rearLeft, + Measure> rearRight) { + this( + frontLeft.in(MetersPerSecond), + frontRight.in(MetersPerSecond), + rearLeft.in(MetersPerSecond), + rearRight.in(MetersPerSecond)); + } + /** * Renormalizes the wheel speeds if any individual speed is above the specified maximum. * @@ -73,6 +98,20 @@ public class MecanumDriveWheelSpeeds { } } + /** + * Renormalizes the wheel speeds if any individual speed is above the specified maximum. + * + *

Sometimes, after inverse kinematics, the requested speed from one or more wheels may be + * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can + * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the + * absolute threshold, while maintaining the ratio of speeds between wheels. + * + * @param attainableMaxSpeed The absolute max speed that a wheel can reach. + */ + public void desaturate(Measure> attainableMaxSpeed) { + desaturate(attainableMaxSpeed.in(MetersPerSecond)); + } + /** * Adds two MecanumDriveWheelSpeeds and returns the sum. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index aa5338e6eb..2498b33742 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -4,11 +4,18 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; + import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import java.util.Arrays; import org.ejml.simple.SimpleMatrix; @@ -302,6 +309,23 @@ public class SwerveDriveKinematics } } + /** + * Renormalizes the wheel speeds if any individual speed is above the specified maximum. + * + *

Sometimes, after inverse kinematics, the requested speed from one or more modules may be + * above the max attainable speed for the driving motor on that module. To fix this issue, one can + * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the + * absolute threshold, while maintaining the ratio of speeds between modules. + * + * @param moduleStates Reference to array of module states. The array will be mutated with the + * normalized speeds! + * @param attainableMaxSpeed The absolute max speed that a module can reach. + */ + public static void desaturateWheelSpeeds( + SwerveModuleState[] moduleStates, Measure> attainableMaxSpeed) { + desaturateWheelSpeeds(moduleStates, attainableMaxSpeed.in(MetersPerSecond)); + } + /** * Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well * as getting rid of joystick saturation at edges of joystick. @@ -348,4 +372,36 @@ public class SwerveDriveKinematics moduleState.speedMetersPerSecond *= scale; } } + + /** + * Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well + * as getting rid of joystick saturation at edges of joystick. + * + *

Sometimes, after inverse kinematics, the requested speed from one or more modules may be + * above the max attainable speed for the driving motor on that module. To fix this issue, one can + * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the + * absolute threshold, while maintaining the ratio of speeds between modules. + * + * @param moduleStates Reference to array of module states. The array will be mutated with the + * normalized speeds! + * @param desiredChassisSpeed The desired speed of the robot + * @param attainableMaxModuleSpeed The absolute max speed that a module can reach + * @param attainableMaxTranslationalSpeed The absolute max speed that your robot can reach while + * translating + * @param attainableMaxRotationalVelocity The absolute max speed the robot can reach while + * rotating + */ + public static void desaturateWheelSpeeds( + SwerveModuleState[] moduleStates, + ChassisSpeeds desiredChassisSpeed, + Measure> attainableMaxModuleSpeed, + Measure> attainableMaxTranslationalSpeed, + Measure> attainableMaxRotationalVelocity) { + desaturateWheelSpeeds( + moduleStates, + desiredChassisSpeed, + attainableMaxModuleSpeed.in(MetersPerSecond), + attainableMaxTranslationalSpeed.in(MetersPerSecond), + attainableMaxRotationalVelocity.in(RadiansPerSecond)); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java index d058764f3b..58467ce313 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java @@ -4,9 +4,13 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import java.util.Objects; /** Represents the state of one swerve module. */ @@ -32,6 +36,16 @@ public class SwerveModulePosition this.angle = angle; } + /** + * Constructs a SwerveModulePosition. + * + * @param distance The distance measured by the wheel of the module. + * @param angle The angle of the module. + */ + public SwerveModulePosition(Measure distance, Rotation2d angle) { + this(distance.in(Meters), angle); + } + @Override public boolean equals(Object obj) { if (obj instanceof SwerveModulePosition) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java index 10dee4fea1..e4529d6b9d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java @@ -4,7 +4,12 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.MetersPerSecond; + import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import java.util.Objects; /** Represents the state of one swerve module. */ @@ -29,6 +34,16 @@ public class SwerveModuleState implements Comparable { this.angle = angle; } + /** + * Constructs a SwerveModuleState. + * + * @param speed The speed of the wheel of the module. + * @param angle The angle of the module. + */ + public SwerveModuleState(Measure> speed, Rotation2d angle) { + this(speed.in(MetersPerSecond), angle); + } + @Override public boolean equals(Object obj) { if (obj instanceof SwerveModuleState) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java index fbf734fd31..931efe5aab 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java @@ -4,6 +4,9 @@ package edu.wpi.first.math.trajectory; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; + import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.MecanumDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -11,6 +14,9 @@ import edu.wpi.first.math.trajectory.constraint.DifferentialDriveKinematicsConst import edu.wpi.first.math.trajectory.constraint.MecanumDriveKinematicsConstraint; import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint; import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import java.util.ArrayList; import java.util.List; @@ -43,6 +49,18 @@ public class TrajectoryConfig { m_constraints = new ArrayList<>(); } + /** + * Constructs the trajectory configuration class. + * + * @param maxVelocity The max velocity for the trajectory. + * @param maxAcceleration The max acceleration for the trajectory. + */ + public TrajectoryConfig( + Measure> maxVelocity, + Measure>> maxAcceleration) { + this(maxVelocity.in(MetersPerSecond), maxAcceleration.in(MetersPerSecondPerSecond)); + } + /** * Adds a user-defined constraint to the trajectory. * @@ -121,6 +139,16 @@ public class TrajectoryConfig { return this; } + /** + * Sets the start velocity of the trajectory. + * + * @param startVelocity The start velocity of the trajectory. + * @return Instance of the current config object. + */ + public TrajectoryConfig setStartVelocity(Measure> startVelocity) { + return setStartVelocity(startVelocity.in(MetersPerSecond)); + } + /** * Returns the starting velocity of the trajectory. * @@ -141,6 +169,16 @@ public class TrajectoryConfig { return this; } + /** + * Sets the end velocity of the trajectory. + * + * @param endVelocity The end velocity of the trajectory. + * @return Instance of the current config object. + */ + public TrajectoryConfig setEndVelocity(Measure> endVelocity) { + return setEndVelocity(endVelocity.in(MetersPerSecond)); + } + /** * Returns the maximum velocity of the trajectory. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java index d29ed107c9..f14aea78f0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java @@ -6,6 +6,9 @@ package edu.wpi.first.math.trajectory; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Unit; +import edu.wpi.first.units.Velocity; import java.util.Objects; /** @@ -67,6 +70,11 @@ public class TrapezoidProfile { this.maxAcceleration = maxAcceleration; MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1); } + + public > Constraints( + Measure> maxVelocity, Measure>> maxAcceleration) { + this(maxVelocity.baseUnitMagnitude(), maxAcceleration.baseUnitMagnitude()); + } } public static class State { @@ -81,6 +89,10 @@ public class TrapezoidProfile { this.velocity = velocity; } + public > State(Measure position, Measure> velocity) { + this(position.baseUnitMagnitude(), velocity.baseUnitMagnitude()); + } + @Override public boolean equals(Object other) { if (other instanceof State) { diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java index d57362d349..2c683c23ba 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java @@ -8,12 +8,22 @@ import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; +import edu.wpi.first.units.Units; import java.util.List; import org.junit.jupiter.api.Test; class Pose2dTest { private static final double kEpsilon = 1E-9; + @Test + void testNewWithMeasures() { + var pose = new Pose2d(Units.Inches.of(6), Units.Inches.of(8), Rotation2d.fromDegrees(45)); + + assertEquals(0.1524, pose.getX(), kEpsilon); + assertEquals(0.2032, pose.getY(), kEpsilon); + assertEquals(Math.PI / 4, pose.getRotation().getRadians(), kEpsilon); + } + @Test void testRotateBy() { final double x = 1.0; diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java index 6702f1d186..7f3e469fad 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java @@ -8,11 +8,19 @@ import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; +import edu.wpi.first.units.Units; import org.junit.jupiter.api.Test; class Rotation2dTest { private static final double kEpsilon = 1E-9; + @Test + void testNewWithMeasures() { + var rot = new Rotation2d(Units.Degrees.of(45)); + + assertEquals(Math.PI / 4, rot.getRadians(), kEpsilon); + } + @Test void testRadiansToDegrees() { var rot1 = Rotation2d.fromRadians(Math.PI / 3); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java index 93d8a0e9af..5817f4e282 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java @@ -6,9 +6,22 @@ package edu.wpi.first.math.geometry; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.units.Units; import org.junit.jupiter.api.Test; class Transform2dTest { + private static final double kEpsilon = 1E-9; + + @Test + void testNewWithMeasures() { + var transform = + new Transform2d(Units.Inches.of(6), Units.Inches.of(8), Rotation2d.fromDegrees(45)); + + assertEquals(0.1524, transform.getX(), kEpsilon); + assertEquals(0.2032, transform.getY(), kEpsilon); + assertEquals(Math.PI / 4, transform.getRotation().getRadians(), kEpsilon); + } + @Test void testInverse() { var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java index 762425b543..cd68dbec77 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java @@ -4,6 +4,7 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Inches; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; @@ -15,6 +16,15 @@ import org.junit.jupiter.api.Test; class Translation3dTest { private static final double kEpsilon = 1E-9; + @Test + void testNewWithMeasures() { + var translation = new Translation3d(Inches.of(6), Inches.of(8), Inches.of(16)); + + assertEquals(0.1524, translation.getX(), kEpsilon); + assertEquals(0.2032, translation.getY(), kEpsilon); + assertEquals(0.4064, translation.getZ(), kEpsilon); + } + @Test void testSum() { var one = new Translation3d(1.0, 3.0, 5.0); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java index 24013427bf..53af407759 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.RPM; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; @@ -44,6 +46,19 @@ class ChassisSpeedsTest { kEpsilon)); } + @Test + void testMeasureConstructor() { + var vx = InchesPerSecond.of(14.52); + var vy = InchesPerSecond.zero(); + var omega = RPM.of(0.02); + var speeds = new ChassisSpeeds(vx, vy, omega); + + assertAll( + () -> assertEquals(0.368808, speeds.vxMetersPerSecond, kEpsilon), + () -> assertEquals(0, speeds.vyMetersPerSecond, kEpsilon), + () -> assertEquals(0.002094395102, speeds.omegaRadiansPerSecond, kEpsilon)); + } + @Test void testFromFieldRelativeSpeeds() { final var chassisSpeeds = diff --git a/wpimath/wpimath-config.cmake.in b/wpimath/wpimath-config.cmake.in index 9100d7943d..8697dc1836 100644 --- a/wpimath/wpimath-config.cmake.in +++ b/wpimath/wpimath-config.cmake.in @@ -1,6 +1,7 @@ include(CMakeFindDependencyMacro) @FILENAME_DEP_REPLACE@ @WPIUTIL_DEP_REPLACE@ +@WPIUNITS_DEP_REPLACE@ if(@USE_SYSTEM_EIGEN@) find_dependency(Eigen3) diff --git a/wpiunits/CMakeLists.txt b/wpiunits/CMakeLists.txt new file mode 100644 index 0000000000..7a5735edcb --- /dev/null +++ b/wpiunits/CMakeLists.txt @@ -0,0 +1,25 @@ +project (wpiunits) + +# Java bindings +if (WITH_JAVA) + find_package(Java REQUIRED) + include(UseJava) + set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") + + file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) + + add_jar(wpiunits_jar ${JAVA_SOURCES} OUTPUT_NAME wpiunits) + + get_property(WPIUNITS_JAR_FILE TARGET wpiunits_jar PROPERTY JAR_FILE) + install(FILES ${WPIUNITS_JAR_FILE} DESTINATION "${java_lib_dest}") + + set_property(TARGET wpiunits_jar PROPERTY FOLDER "java") + + if (WITH_FLAT_INSTALL) + set (wpiunits_config_dir ${wpiunits_dest}) + else() + set (wpiunits_config_dir share/wpiunits) + endif() + + install(FILES wpiunits-config.cmake DESTINATION ${wpiunits_config_dir}) +endif() diff --git a/wpiunits/src/main/java/edu/wpi/first/units/BaseUnits.java b/wpiunits/src/main/java/edu/wpi/first/units/BaseUnits.java index 5e342da121..facf02540d 100644 --- a/wpiunits/src/main/java/edu/wpi/first/units/BaseUnits.java +++ b/wpiunits/src/main/java/edu/wpi/first/units/BaseUnits.java @@ -20,11 +20,11 @@ public final class BaseUnits { public static final Velocity Velocity = new Velocity<>(Distance, Time, "Meter per Second", "m/s"); - /** The standard unit of mass, grams. */ + /** The standard unit of mass, kilograms. */ public static final Mass Mass = new Mass(1, "Kilogram", "Kg"); - /** The standard unit of angles, revolutions. */ - public static final Angle Angle = new Angle(1, "Revolution", "R"); + /** The standard unit of angles, radians. */ + public static final Angle Angle = new Angle(1, "Radian", "rad"); /** The standard "unitless" unit. */ public static final Dimensionless Value = new Dimensionless(1, "", ""); diff --git a/wpiunits/src/main/java/edu/wpi/first/units/Units.java b/wpiunits/src/main/java/edu/wpi/first/units/Units.java index ecf0768a18..a04b54cc08 100644 --- a/wpiunits/src/main/java/edu/wpi/first/units/Units.java +++ b/wpiunits/src/main/java/edu/wpi/first/units/Units.java @@ -38,10 +38,9 @@ public final class Units { public static final Time Minute = Minutes; // singularized alias // Angle - public static final Angle Revolutions = BaseUnits.Angle; - public static final Angle Rotations = new Angle(1, "Rotation", "R"); // alias - public static final Angle Radians = - derive(Revolutions).splitInto(2 * Math.PI).named("Radian").symbol("rad").make(); + public static final Angle Radians = BaseUnits.Angle; + public static final Angle Revolutions = new Angle(2 * Math.PI, "Revolution", "R"); + public static final Angle Rotations = new Angle(2 * Math.PI, "Rotation", "R"); // alias revolution public static final Angle Degrees = derive(Revolutions).splitInto(360).named("Degree").symbol("°").make(); diff --git a/wpiunits/src/test/java/edu/wpi/first/units/UnitsTest.java b/wpiunits/src/test/java/edu/wpi/first/units/UnitsTest.java index 46591118ab..c428cad8ac 100644 --- a/wpiunits/src/test/java/edu/wpi/first/units/UnitsTest.java +++ b/wpiunits/src/test/java/edu/wpi/first/units/UnitsTest.java @@ -203,13 +203,14 @@ class UnitsTest { @Test void testRevolutions() { - testBaseUnit(Revolutions); + assertEquals(1, Revolutions.convertFrom(2 * Math.PI, Radians), thresh); assertEquals("Revolution", Revolutions.name()); assertEquals("R", Revolutions.symbol()); } @Test void testRadians() { + testBaseUnit(Radians); assertEquals(2 * Math.PI, Radians.convertFrom(1, Revolutions), thresh); assertEquals(2 * Math.PI, Radians.convertFrom(360, Degrees), thresh); assertEquals("Radian", Radians.name()); diff --git a/wpiunits/wpiunits-config.cmake b/wpiunits/wpiunits-config.cmake new file mode 100644 index 0000000000..e04a6bc9de --- /dev/null +++ b/wpiunits/wpiunits-config.cmake @@ -0,0 +1,2 @@ +get_filename_component(SELF_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) +include(${SELF_DIR}/wpiunits.cmake) diff --git a/xrpVendordep/CMakeLists.txt b/xrpVendordep/CMakeLists.txt index 40d40a7131..2688fe0626 100644 --- a/xrpVendordep/CMakeLists.txt +++ b/xrpVendordep/CMakeLists.txt @@ -10,7 +10,7 @@ if (WITH_JAVA) set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) - add_jar(xrpVendordep_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME xrpVendordep) + add_jar(xrpVendordep_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiunits_jar wpiutil_jar wpilibj_jar OUTPUT_NAME xrpVendordep) get_property(xrpVendordep_JAR_FILE TARGET xrpVendordep_jar PROPERTY JAR_FILE) install(FILES ${xrpVendordep_JAR_FILE} DESTINATION "${java_lib_dest}")