mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-25 01:41:43 +00:00
[wpimath] Add 3D odometry and pose estimation (#7119)
This commit is contained in:
Joseph Eng
@@ -0,0 +1,149 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.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.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.units.measure.Distance;
|
||||
|
||||
/**
|
||||
* Class for differential drive odometry. Odometry allows you to track the robot's position on the
|
||||
* field over the course of a match using readings from 2 encoders and a gyroscope.
|
||||
*
|
||||
* <p>This class is meant to be an easy replacement for {@link DifferentialDriveOdometry}, only
|
||||
* requiring the addition of appropriate conversions between 2D and 3D versions of geometry classes.
|
||||
* (See {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
|
||||
* Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
|
||||
*
|
||||
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
|
||||
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
|
||||
*
|
||||
* <p>It is important that you reset your encoders to zero before using this class. Any subsequent
|
||||
* pose resets also require the encoders to be reset to zero.
|
||||
*/
|
||||
public class DifferentialDriveOdometry3d extends Odometry3d<DifferentialDriveWheelPositions> {
|
||||
/**
|
||||
* Constructs a DifferentialDriveOdometry3d object.
|
||||
*
|
||||
* @param gyroAngle The angle reported by the gyroscope.
|
||||
* @param leftDistanceMeters The distance traveled by the left encoder.
|
||||
* @param rightDistanceMeters The distance traveled by the right encoder.
|
||||
* @param initialPoseMeters The starting position of the robot on the field.
|
||||
*/
|
||||
public DifferentialDriveOdometry3d(
|
||||
Rotation3d gyroAngle,
|
||||
double leftDistanceMeters,
|
||||
double rightDistanceMeters,
|
||||
Pose3d initialPoseMeters) {
|
||||
super(
|
||||
new DifferentialDriveKinematics(1),
|
||||
gyroAngle,
|
||||
new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
|
||||
initialPoseMeters);
|
||||
MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a DifferentialDriveOdometry3d object.
|
||||
*
|
||||
* @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 initialPoseMeters The starting position of the robot on the field.
|
||||
*/
|
||||
public DifferentialDriveOdometry3d(
|
||||
Rotation3d gyroAngle,
|
||||
Distance leftDistance,
|
||||
Distance rightDistance,
|
||||
Pose3d initialPoseMeters) {
|
||||
this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a DifferentialDriveOdometry3d object.
|
||||
*
|
||||
* @param gyroAngle The angle reported by the gyroscope.
|
||||
* @param leftDistanceMeters The distance traveled by the left encoder.
|
||||
* @param rightDistanceMeters The distance traveled by the right encoder.
|
||||
*/
|
||||
public DifferentialDriveOdometry3d(
|
||||
Rotation3d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
|
||||
this(gyroAngle, leftDistanceMeters, rightDistanceMeters, Pose3d.kZero);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a DifferentialDriveOdometry3d object.
|
||||
*
|
||||
* @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.
|
||||
*/
|
||||
public DifferentialDriveOdometry3d(
|
||||
Rotation3d gyroAngle, Distance leftDistance, Distance rightDistance) {
|
||||
this(gyroAngle, leftDistance, rightDistance, Pose3d.kZero);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the robot's position on the field.
|
||||
*
|
||||
* <p>The 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 leftDistanceMeters The distance traveled by the left encoder.
|
||||
* @param rightDistanceMeters The distance traveled by the right encoder.
|
||||
* @param poseMeters The position on the field that your robot is at.
|
||||
*/
|
||||
public void resetPosition(
|
||||
Rotation3d gyroAngle,
|
||||
double leftDistanceMeters,
|
||||
double rightDistanceMeters,
|
||||
Pose3d poseMeters) {
|
||||
super.resetPosition(
|
||||
gyroAngle,
|
||||
new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
|
||||
poseMeters);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the robot's position on the field.
|
||||
*
|
||||
* <p>The 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(
|
||||
Rotation3d gyroAngle, Distance leftDistance, Distance rightDistance, Pose3d 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
|
||||
* advantageous for teams that are using lower CPR encoders.
|
||||
*
|
||||
* @param gyroAngle The angle reported by the gyroscope.
|
||||
* @param leftDistanceMeters The distance traveled by the left encoder.
|
||||
* @param rightDistanceMeters The distance traveled by the right encoder.
|
||||
* @return The new pose of the robot.
|
||||
*/
|
||||
public Pose3d update(
|
||||
Rotation3d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
|
||||
return super.update(
|
||||
gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.kinematics;
|
||||
|
||||
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.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
|
||||
/**
|
||||
* Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
|
||||
* over a course of a match using readings from your mecanum wheel encoders.
|
||||
*
|
||||
* <p>This class is meant to be an easy replacement for {@link MecanumDriveOdometry}, only requiring
|
||||
* the addition of appropriate conversions between 2D and 3D versions of geometry classes. (See
|
||||
* {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
|
||||
* Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
|
||||
*
|
||||
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
|
||||
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
|
||||
*/
|
||||
public class MecanumDriveOdometry3d extends Odometry3d<MecanumDriveWheelPositions> {
|
||||
/**
|
||||
* Constructs a MecanumDriveOdometry3d object.
|
||||
*
|
||||
* @param kinematics The mecanum drive kinematics for your drivetrain.
|
||||
* @param gyroAngle The angle reported by the gyroscope.
|
||||
* @param wheelPositions The distances driven by each wheel.
|
||||
* @param initialPoseMeters The starting position of the robot on the field.
|
||||
*/
|
||||
public MecanumDriveOdometry3d(
|
||||
MecanumDriveKinematics kinematics,
|
||||
Rotation3d gyroAngle,
|
||||
MecanumDriveWheelPositions wheelPositions,
|
||||
Pose3d initialPoseMeters) {
|
||||
super(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
|
||||
MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a MecanumDriveOdometry3d object with the default pose at the origin.
|
||||
*
|
||||
* @param kinematics The mecanum drive kinematics for your drivetrain.
|
||||
* @param gyroAngle The angle reported by the gyroscope.
|
||||
* @param wheelPositions The distances driven by each wheel.
|
||||
*/
|
||||
public MecanumDriveOdometry3d(
|
||||
MecanumDriveKinematics kinematics,
|
||||
Rotation3d gyroAngle,
|
||||
MecanumDriveWheelPositions wheelPositions) {
|
||||
this(kinematics, gyroAngle, wheelPositions, Pose3d.kZero);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,148 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.kinematics;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.math.geometry.Twist3d;
|
||||
|
||||
/**
|
||||
* Class for odometry. Robot code should not use this directly- Instead, use the particular type for
|
||||
* your drivetrain (e.g., {@link DifferentialDriveOdometry3d}). Odometry allows you to track the
|
||||
* robot's position on the field over the course of a match using readings from encoders and a
|
||||
* gyroscope.
|
||||
*
|
||||
* <p>This class is meant to be an easy replacement for {@link Odometry}, only requiring the
|
||||
* addition of appropriate conversions between 2D and 3D versions of geometry classes. (See {@link
|
||||
* Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
|
||||
* Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
|
||||
*
|
||||
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
|
||||
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
|
||||
*
|
||||
* @param <T> Wheel positions type.
|
||||
*/
|
||||
public class Odometry3d<T> {
|
||||
private final Kinematics<?, T> m_kinematics;
|
||||
private Pose3d m_poseMeters;
|
||||
|
||||
private Rotation3d m_gyroOffset;
|
||||
private Rotation3d m_previousAngle;
|
||||
private final T m_previousWheelPositions;
|
||||
|
||||
/**
|
||||
* Constructs an Odometry3d object.
|
||||
*
|
||||
* @param kinematics The kinematics of the drivebase.
|
||||
* @param gyroAngle The angle reported by the gyroscope.
|
||||
* @param wheelPositions The current encoder readings.
|
||||
* @param initialPoseMeters The starting position of the robot on the field.
|
||||
*/
|
||||
public Odometry3d(
|
||||
Kinematics<?, T> kinematics,
|
||||
Rotation3d gyroAngle,
|
||||
T wheelPositions,
|
||||
Pose3d initialPoseMeters) {
|
||||
m_kinematics = kinematics;
|
||||
m_poseMeters = initialPoseMeters;
|
||||
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
|
||||
m_previousAngle = m_poseMeters.getRotation();
|
||||
m_previousWheelPositions = m_kinematics.copy(wheelPositions);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the robot's position on the field.
|
||||
*
|
||||
* <p>The 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 wheelPositions The current encoder readings.
|
||||
* @param poseMeters The position on the field that your robot is at.
|
||||
*/
|
||||
public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) {
|
||||
m_poseMeters = poseMeters;
|
||||
m_previousAngle = m_poseMeters.getRotation();
|
||||
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
|
||||
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the pose.
|
||||
*
|
||||
* @param poseMeters The pose to reset to.
|
||||
*/
|
||||
public void resetPose(Pose3d poseMeters) {
|
||||
m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation()));
|
||||
m_poseMeters = poseMeters;
|
||||
m_previousAngle = m_poseMeters.getRotation();
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the translation of the pose.
|
||||
*
|
||||
* @param translation The translation to reset to.
|
||||
*/
|
||||
public void resetTranslation(Translation3d translation) {
|
||||
m_poseMeters = new Pose3d(translation, m_poseMeters.getRotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the rotation of the pose.
|
||||
*
|
||||
* @param rotation The rotation to reset to.
|
||||
*/
|
||||
public void resetRotation(Rotation3d rotation) {
|
||||
m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation()));
|
||||
m_poseMeters = new Pose3d(m_poseMeters.getTranslation(), rotation);
|
||||
m_previousAngle = m_poseMeters.getRotation();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the position of the robot on the field.
|
||||
*
|
||||
* @return The pose of the robot (x, y, and z are in meters).
|
||||
*/
|
||||
public Pose3d getPoseMeters() {
|
||||
return m_poseMeters;
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the robot's position on the field using forward kinematics and integration of the pose
|
||||
* over time. This method takes in an angle parameter which is used instead of the angular rate
|
||||
* that is calculated from forward kinematics, in addition to the current distance measurement at
|
||||
* each wheel.
|
||||
*
|
||||
* @param gyroAngle The angle reported by the gyroscope.
|
||||
* @param wheelPositions The current encoder readings.
|
||||
* @return The new pose of the robot.
|
||||
*/
|
||||
public Pose3d update(Rotation3d gyroAngle, T wheelPositions) {
|
||||
var angle = gyroAngle.plus(m_gyroOffset);
|
||||
var angle_difference = angle.minus(m_previousAngle).getQuaternion().toRotationVector();
|
||||
|
||||
var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
|
||||
var twist =
|
||||
new Twist3d(
|
||||
twist2d.dx,
|
||||
twist2d.dy,
|
||||
0,
|
||||
angle_difference.get(0),
|
||||
angle_difference.get(1),
|
||||
angle_difference.get(2));
|
||||
|
||||
var newPose = m_poseMeters.exp(twist);
|
||||
|
||||
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
|
||||
m_previousAngle = angle;
|
||||
m_poseMeters = new Pose3d(newPose.getTranslation(), angle);
|
||||
|
||||
return m_poseMeters;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.kinematics;
|
||||
|
||||
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.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
|
||||
/**
|
||||
* Class for swerve drive odometry. Odometry allows you to track the robot's position on the field
|
||||
* over a course of a match using readings from your swerve drive encoders and swerve azimuth
|
||||
* encoders.
|
||||
*
|
||||
* <p>This class is meant to be an easy replacement for {@link SwerveDriveOdometry}, only requiring
|
||||
* the addition of appropriate conversions between 2D and 3D versions of geometry classes. (See
|
||||
* {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
|
||||
* Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
|
||||
*
|
||||
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
|
||||
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
|
||||
*/
|
||||
public class SwerveDriveOdometry3d extends Odometry3d<SwerveModulePosition[]> {
|
||||
private final int m_numModules;
|
||||
|
||||
/**
|
||||
* Constructs a SwerveDriveOdometry3d object.
|
||||
*
|
||||
* @param kinematics The swerve drive kinematics for your drivetrain.
|
||||
* @param gyroAngle The angle reported by the gyroscope.
|
||||
* @param modulePositions The wheel positions reported by each module.
|
||||
* @param initialPose The starting position of the robot on the field.
|
||||
*/
|
||||
public SwerveDriveOdometry3d(
|
||||
SwerveDriveKinematics kinematics,
|
||||
Rotation3d gyroAngle,
|
||||
SwerveModulePosition[] modulePositions,
|
||||
Pose3d initialPose) {
|
||||
super(kinematics, gyroAngle, modulePositions, initialPose);
|
||||
|
||||
m_numModules = modulePositions.length;
|
||||
|
||||
MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a SwerveDriveOdometry3d object with the default pose at the origin.
|
||||
*
|
||||
* @param kinematics The swerve drive kinematics for your drivetrain.
|
||||
* @param gyroAngle The angle reported by the gyroscope.
|
||||
* @param modulePositions The wheel positions reported by each module.
|
||||
*/
|
||||
public SwerveDriveOdometry3d(
|
||||
SwerveDriveKinematics kinematics,
|
||||
Rotation3d gyroAngle,
|
||||
SwerveModulePosition[] modulePositions) {
|
||||
this(kinematics, gyroAngle, modulePositions, Pose3d.kZero);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void resetPosition(
|
||||
Rotation3d gyroAngle, SwerveModulePosition[] modulePositions, Pose3d pose) {
|
||||
if (modulePositions.length != m_numModules) {
|
||||
throw new IllegalArgumentException(
|
||||
"Number of modules is not consistent with number of wheel locations provided in "
|
||||
+ "constructor");
|
||||
}
|
||||
super.resetPosition(gyroAngle, modulePositions, pose);
|
||||
}
|
||||
|
||||
@Override
|
||||
public Pose3d update(Rotation3d gyroAngle, SwerveModulePosition[] modulePositions) {
|
||||
if (modulePositions.length != m_numModules) {
|
||||
throw new IllegalArgumentException(
|
||||
"Number of modules is not consistent with number of wheel locations provided in "
|
||||
+ "constructor");
|
||||
}
|
||||
return super.update(gyroAngle, modulePositions);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user