Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-11-08 00:03:50 -08:00
parent 2109161534 b8d6bc2eb1
commit 161f8d107b
6 changed files with 90 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java
View File

@@ -9,8 +9,8 @@ import java.util.Optional;
import java.util.TreeMap;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Twist2d;
import org.wpilib.math.interpolation.TimeInterpolatableBuffer;
import org.wpilib.math.kinematics.Kinematics;
import org.wpilib.math.kinematics.Odometry;
@@ -270,20 +270,27 @@ public class PoseEstimator<T> {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision pose.
var twist = visionRobotPose.minus(visionSample.get()).log();
// Step 4: Measure the transform between the old pose estimate and the vision pose.
var transform = visionRobotPose.minus(visionSample.get());
// Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// Step 5: We should not trust the transform entirely, so instead we scale this transform by a
// Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
var k_times_transform =
m_visionK.times(
VecBuilder.fill(
transform.getX(), transform.getY(), transform.getRotation().getRadians()));
// Step 6: Convert back to Twist2d.
var scaledTwist =
new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
// Step 6: Convert back to Transform2d.
var scaledTransform =
new Transform2d(
k_times_transform.get(0, 0),
k_times_transform.get(1, 0),
Rotation2d.fromRadians(k_times_transform.get(2, 0)));
// Step 7: Calculate and record the vision update.
var visionUpdate =
new VisionUpdate(visionSample.get().plus(scaledTwist.exp()), odometrySample.get());
new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get());
m_visionUpdates.put(timestamp, visionUpdate);
// Step 8: Remove later vision measurements. (Matches previous behavior)

40
wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java
View File

@@ -11,9 +11,9 @@ import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.geometry.Twist3d;
import org.wpilib.math.interpolation.TimeInterpolatableBuffer;
import org.wpilib.math.kinematics.Kinematics;
import org.wpilib.math.kinematics.Odometry3d;
@@ -282,28 +282,36 @@ public class PoseEstimator3d<T> {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision pose.
var twist = visionRobotPose.minus(visionSample.get()).log();
// Step 4: Measure the transform between the old pose estimate and the vision pose.
var transform = visionRobotPose.minus(visionSample.get());
// Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// Step 5: We should not trust the transform entirely, so instead we scale this transform by a
// Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
var k_times_twist =
var k_times_transform =
m_visionK.times(
VecBuilder.fill(twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz));
VecBuilder.fill(
transform.getX(),
transform.getY(),
transform.getZ(),
transform.getRotation().getX(),
transform.getRotation().getY(),
transform.getRotation().getZ()));
// Step 6: Convert back to Twist3d.
var scaledTwist =
new Twist3d(
k_times_twist.get(0, 0),
k_times_twist.get(1, 0),
k_times_twist.get(2, 0),
k_times_twist.get(3, 0),
k_times_twist.get(4, 0),
k_times_twist.get(5, 0));
// Step 6: Convert back to Transform3d.
var scaledTransform =
new Transform3d(
k_times_transform.get(0, 0),
k_times_transform.get(1, 0),
k_times_transform.get(2, 0),
new Rotation3d(
k_times_transform.get(3, 0),
k_times_transform.get(4, 0),
k_times_transform.get(5, 0)));
// Step 7: Calculate and record the vision update.
var visionUpdate =
new VisionUpdate(visionSample.get().plus(scaledTwist.exp()), odometrySample.get());
new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get());
m_visionUpdates.put(timestamp, visionUpdate);
// Step 8: Remove later vision measurements. (Matches previous behavior)

32
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp
View File

@@ -12,6 +12,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/math/geometry/Transform2d.hpp"
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/math/interpolation/TimeInterpolatableBuffer.hpp"
#include "wpi/math/kinematics/Kinematics.hpp"
@@ -261,25 +262,26 @@ class WPILIB_DLLEXPORT PoseEstimator {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision
// pose.
auto twist = (visionRobotPose - visionSample.value()).Log();
// Step 4: Measure the transform between the old pose estimate and the
// vision transform.
auto transform = visionRobotPose - visionSample.value();
// Step 5: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
Eigen::Vector3d k_times_twist =
m_visionK * Eigen::Vector3d{twist.dx.value(), twist.dy.value(),
twist.dtheta.value()};
// Step 5: We should not trust the transform entirely, so instead we scale
// this transform by a Kalman gain matrix representing how much we trust
// vision measurements compared to our current pose.
Eigen::Vector3d k_times_transform =
m_visionK * Eigen::Vector3d{transform.X().value(),
transform.Y().value(),
transform.Rotation().Radians().value()};
// Step 6: Convert back to Twist2d.
Twist2d scaledTwist{wpi::units::meter_t{k_times_twist(0)},
wpi::units::meter_t{k_times_twist(1)},
wpi::units::radian_t{k_times_twist(2)}};
// Step 6: Convert back to Transform2d.
Transform2d scaledTransform{
wpi::units::meter_t{k_times_transform(0)},
wpi::units::meter_t{k_times_transform(1)},
Rotation2d{wpi::units::radian_t{k_times_transform(2)}}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),
*odometrySample};
VisionUpdate visionUpdate{*visionSample + scaledTransform, *odometrySample};
m_visionUpdates[timestamp] = visionUpdate;
// Step 8: Remove later vision measurements. (Matches previous behavior)

42
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp
View File

@@ -13,6 +13,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/math/geometry/Transform3d.hpp"
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/math/interpolation/TimeInterpolatableBuffer.hpp"
#include "wpi/math/kinematics/Kinematics.hpp"
@@ -270,29 +271,32 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision
// pose.
auto twist = (visionRobotPose - visionSample.value()).Log();
// Step 4: Measure the transform between the old pose estimate and the
// vision pose.
auto transform = visionRobotPose - visionSample.value();
// Step 5: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
wpi::math::Vectord<6> k_times_twist =
m_visionK * wpi::math::Vectord<6>{twist.dx.value(), twist.dy.value(),
twist.dz.value(), twist.rx.value(),
twist.ry.value(), twist.rz.value()};
// Step 5: We should not trust the transform entirely, so instead we scale
// this transform by a Kalman gain matrix representing how much we trust
// vision measurements compared to our current pose.
wpi::math::Vectord<6> k_times_transform =
m_visionK * wpi::math::Vectord<6>{transform.X().value(),
transform.Y().value(),
transform.Z().value(),
transform.Rotation().X().value(),
transform.Rotation().Y().value(),
transform.Rotation().Z().value()};
// Step 6: Convert back to Twist3d.
Twist3d scaledTwist{wpi::units::meter_t{k_times_twist(0)},
wpi::units::meter_t{k_times_twist(1)},
wpi::units::meter_t{k_times_twist(2)},
wpi::units::radian_t{k_times_twist(3)},
wpi::units::radian_t{k_times_twist(4)},
wpi::units::radian_t{k_times_twist(5)}};
// Step 6: Convert back to Transform3d.
Transform3d scaledTransform{
wpi::units::meter_t{k_times_transform(0)},
wpi::units::meter_t{k_times_transform(1)},
wpi::units::meter_t{k_times_transform(2)},
Rotation3d{wpi::units::radian_t{k_times_transform(3)},
wpi::units::radian_t{k_times_transform(4)},
wpi::units::radian_t{k_times_transform(5)}}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),
*odometrySample};
VisionUpdate visionUpdate{*visionSample + scaledTransform, *odometrySample};
m_visionUpdates[timestamp] = visionUpdate;
// Step 8: Remove later vision measurements. (Matches previous behavior)