Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[wpimath] Add Reset methods to PoseEstimator #6751

Merged
merged 2 commits into from
Jul 9, 2024
Merged

[wpimath] Add Reset methods to PoseEstimator #6751

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8a030e0
[wpimath] Add reset methods to Odometry
spacey-sooty Jun 5, 2024
15c818f
[wpimath] Add Reset methods to PoseEstimator
spacey-sooty Jun 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 31 additions & 0 deletions wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
import edu.wpi.first.math.VecBuilder;
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.math.geometry.Twist2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.Kinematics;
Expand Down Expand Up @@ -125,6 +126,36 @@ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet
m_poseEstimate = m_odometry.getPoseMeters();
}

/**
* Resets the robot's pose.
*
* @param pose The pose to reset to.
*/
public void resetPose(Pose2d pose) {
m_odometry.resetPose(pose);
m_odometryPoseBuffer.clear();
}

/**
* Resets the robot's translation.
*
* @param translation The pose to translation to.
*/
public void resetTranslation(Translation2d translation) {
m_odometry.resetTranslation(translation);
m_odometryPoseBuffer.clear();
}

/**
* Resets the robot's rotation.
*
* @param rotation The rotation to reset to.
*/
public void resetRotation(Rotation2d rotation) {
m_odometry.resetRotation(rotation);
m_odometryPoseBuffer.clear();
}

/**
* Gets the estimated robot pose.
*
Expand Down
32 changes: 32 additions & 0 deletions wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;

/**
* Class for odometry. Robot code should not use this directly- Instead, use the particular type for
Expand Down Expand Up @@ -63,6 +64,37 @@ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet
m_previousWheelPositions = m_kinematics.copy(wheelPositions);
}

/**
* Resets the pose.
*
* @param poseMeters The pose to reset to.
*/
public void resetPose(Pose2d 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(Translation2d translation) {
m_poseMeters = new Pose2d(translation, m_poseMeters.getRotation());
}

/**
* Resets the rotation of the pose.
*
* @param rotation The rotation to reset to.
*/
public void resetRotation(Rotation2d rotation) {
m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation()));
m_poseMeters = new Pose2d(m_poseMeters.getTranslation(), rotation);
m_previousAngle = m_poseMeters.getRotation();
}

/**
* Returns the position of the robot on the field.
*
Expand Down
22 changes: 22 additions & 0 deletions wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@

#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/Kinematics.h"
#include "frc/kinematics/Odometry.h"
Expand Down Expand Up @@ -87,6 +88,27 @@ class WPILIB_DLLEXPORT PoseEstimator {
void ResetPosition(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions, const Pose2d& pose);

/**
* Resets the robot's pose.
*
* @param pose The pose to reset to.
*/
void ResetPose(const Pose2d& pose);

/**
* Resets the robot's translation.
*
* @param translation The pose to translation to.
*/
void ResetTranslation(const Translation2d& translation);

/**
* Resets the robot's rotation.
*
* @param rotation The rotation to reset to.
*/
void ResetRotation(const Rotation2d& rotation);

/**
* Gets the estimated robot pose.
*
Expand Down
23 changes: 23 additions & 0 deletions wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,9 @@
#pragma once

#include "frc/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/WheelPositions.h"

namespace frc {

Expand Down Expand Up @@ -53,6 +56,26 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
m_poseEstimate = m_odometry.GetPose();
}

template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPose(const Pose2d& pose) {
m_odometry.ResetPose(pose);
m_odometryPoseBuffer.Clear();
}

template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetTranslation(
const Translation2d& translation) {
m_odometry.ResetTranslation(translation);
m_odometryPoseBuffer.Clear();
}

template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetRotation(
const Rotation2d& rotation) {
m_odometry.ResetTranslation(rotation);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This should be ResetRotation? It would be good if some test was added that instantiated these templates to catch compile errors like this.

m_odometryPoseBuffer.Clear();
}

template <typename WheelSpeeds, typename WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
const {
Expand Down
33 changes: 33 additions & 0 deletions wpimath/src/main/native/include/frc/kinematics/Odometry.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@
#include <wpi/SymbolExports.h>

#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/Kinematics.h"
#include "frc/kinematics/WheelPositions.h"

Expand Down Expand Up @@ -58,6 +60,37 @@ class WPILIB_DLLEXPORT Odometry {
m_previousWheelPositions = wheelPositions;
}

/**
* Resets the pose.
*
* @param pose The pose to reset to.
*/
void ResetPose(const Pose2d& pose) {
m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation());
m_pose = pose;
m_previousAngle = pose.Rotation();
}

/**
* Resets the translation of the pose.
*
* @param translation The translation to reset to.
*/
void ResetTranslation(const Translation2d& translation) {
m_pose = Pose2d{translation, m_pose.Rotation()};
}

/**
* Resets the rotation of the pose.
*
* @param rotation The rotation to reset to.
*/
void ResetRotation(const Rotation2d& rotation) {
m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation());
m_pose = Pose2d{m_pose.Translation(), rotation};
m_previousAngle = rotation;
}

/**
* Returns the position of the robot on the field.
* @return The pose of the robot.
Expand Down
Loading