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}")