Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Change kinematics.ToTwist2d(end - start) to kinematics.ToTwist2d(start, end) (#5493)

Browse Source
This commit is contained in:
Joseph Eng
2023-08-01 22:25:26 -07:00
committed by GitHub
parent 815a8403e5
commit 0c93aded8a
23 changed files with 98 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
View File

@@ -300,14 +300,11 @@ public class PoseEstimator<T extends WheelPositions<T>> {
// Find the new wheel distances.
var wheelLerp = wheelPositions.interpolate(endValue.wheelPositions, t);
// Find the distance travelled between this measurement and the interpolated measurement.
var wheelDelta = wheelLerp.minus(wheelPositions);
// Find the new gyro angle.
var gyroLerp = gyroAngle.interpolate(endValue.gyroAngle, t);
// Create a twist to represent this change based on the interpolated sensor inputs.
Twist2d twist = m_kinematics.toTwist2d(wheelDelta);
// Create a twist to represent the change based on the interpolated sensor inputs.
Twist2d twist = m_kinematics.toTwist2d(wheelPositions, wheelLerp);
twist.dtheta = gyroLerp.minus(gyroAngle).getRadians();
return new InterpolationRecord(poseMeters.exp(twist), gyroLerp, wheelLerp);

5
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
View File

@@ -63,8 +63,9 @@ public class DifferentialDriveKinematics
}
@Override
public Twist2d toTwist2d(DifferentialDriveWheelPositions wheelDeltas) {
return toTwist2d(wheelDeltas.leftMeters, wheelDeltas.rightMeters);
public Twist2d toTwist2d(
DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) {
return toTwist2d(end.leftMeters - start.leftMeters, end.rightMeters - start.rightMeters);
}
/**

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
View File

@@ -52,12 +52,6 @@ public class DifferentialDriveWheelPositions
return new DifferentialDriveWheelPositions(leftMeters, rightMeters);
}
@Override
public DifferentialDriveWheelPositions minus(DifferentialDriveWheelPositions other) {
return new DifferentialDriveWheelPositions(
this.leftMeters - other.leftMeters, this.rightMeters - other.rightMeters);
}
@Override
public DifferentialDriveWheelPositions interpolate(
DifferentialDriveWheelPositions endValue, double t) {

11
wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java
View File

@@ -35,12 +35,13 @@ public interface Kinematics<S, P> {
S toWheelSpeeds(ChassisSpeeds chassisSpeeds);
/**
* Performs forward kinematics to return the resulting from the given wheel deltas. This method is
* often used for odometry -- determining the robot's position on the field using changes in the
* distance driven by each wheel on the robot.
* Performs forward kinematics to return the resulting Twist2d from the given change in wheel
* positions. This method is often used for odometry -- determining the robot's position on the
* field using changes in the distance driven by each wheel on the robot.
*
* @param wheelDeltas The distances driven by each wheel.
* @param start The starting distances driven by the wheels.
* @param end The ending distances driven by the wheels.
* @return The resulting Twist2d in the robot's movement.
*/
Twist2d toTwist2d(P wheelDeltas);
Twist2d toTwist2d(P start, P end);
}

22
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
View File

@@ -158,6 +158,27 @@ public class MecanumDriveKinematics
}
@Override
public Twist2d toTwist2d(MecanumDriveWheelPositions start, MecanumDriveWheelPositions end) {
var wheelDeltasVector = new SimpleMatrix(4, 1);
wheelDeltasVector.setColumn(
0,
0,
end.frontLeftMeters - start.frontLeftMeters,
end.frontRightMeters - start.frontRightMeters,
end.rearLeftMeters - start.rearLeftMeters,
end.rearRightMeters - start.rearRightMeters);
var twist = m_forwardKinematics.mult(wheelDeltasVector);
return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
}
/**
* Performs forward kinematics to return the resulting Twist2d from the given wheel deltas. This
* method is often used for odometry -- determining the robot's position on the field using
* changes in the distance driven by each wheel on the robot.
*
* @param wheelDeltas The distances driven by each wheel.
* @return The resulting Twist2d.
*/
public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas) {
var wheelDeltasVector = new SimpleMatrix(4, 1);
wheelDeltasVector.setColumn(
@@ -168,7 +189,6 @@ public class MecanumDriveKinematics
wheelDeltas.rearLeftMeters,
wheelDeltas.rearRightMeters);
var twist = m_forwardKinematics.mult(wheelDeltasVector);
return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
}

9
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
View File

@@ -73,15 +73,6 @@ public class MecanumDriveWheelPositions implements WheelPositions<MecanumDriveWh
frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
}
@Override
public MecanumDriveWheelPositions minus(MecanumDriveWheelPositions other) {
return new MecanumDriveWheelPositions(
this.frontLeftMeters - other.frontLeftMeters,
this.frontRightMeters - other.frontRightMeters,
this.rearLeftMeters - other.rearLeftMeters,
this.rearRightMeters - other.rearRightMeters);
}
@Override
public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) {
return new MecanumDriveWheelPositions(

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
View File

@@ -81,11 +81,9 @@ public class Odometry<T extends WheelPositions<T>> {
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
T wheelDeltas = wheelPositions.minus(m_previousWheelPositions);
var angle = gyroAngle.plus(m_gyroOffset);
var twist = m_kinematics.toTwist2d(wheelDeltas);
var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
twist.dtheta = angle.minus(m_previousAngle).getRadians();
var newPose = m_poseMeters.exp(twist);

15
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
View File

@@ -261,8 +261,19 @@ public class SwerveDriveKinematics
}
@Override
public Twist2d toTwist2d(SwerveDriveWheelPositions wheelDeltas) {
return toTwist2d(wheelDeltas.positions);
public Twist2d toTwist2d(SwerveDriveWheelPositions start, SwerveDriveWheelPositions end) {
if (start.positions.length != end.positions.length) {
throw new IllegalArgumentException("Inconsistent number of modules!");
}
var newPositions = new SwerveModulePosition[start.positions.length];
for (int i = 0; i < start.positions.length; i++) {
var startModule = start.positions[i];
var endModule = end.positions[i];
newPositions[i] =
new SwerveModulePosition(
endModule.distanceMeters - startModule.distanceMeters, endModule.angle);
}
return toTwist2d(newPositions);
}
/**

27
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java
View File

@@ -6,7 +6,6 @@ package edu.wpi.first.math.kinematics;
import java.util.Arrays;
import java.util.Objects;
import java.util.function.BinaryOperator;
public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWheelPositions> {
public SwerveModulePosition[] positions;
@@ -43,30 +42,20 @@ public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWhee
return String.format("SwerveDriveWheelPositions(%s)", Arrays.toString(positions));
}
private SwerveDriveWheelPositions generate(
SwerveDriveWheelPositions other, BinaryOperator<SwerveModulePosition> generator) {
if (other.positions.length != positions.length) {
throw new IllegalArgumentException("Inconsistent number of modules!");
}
var newPositions = new SwerveModulePosition[positions.length];
for (int i = 0; i < positions.length; i++) {
newPositions[i] = generator.apply(positions[i], other.positions[i]);
}
return new SwerveDriveWheelPositions(newPositions);
}
@Override
public SwerveDriveWheelPositions copy() {
return new SwerveDriveWheelPositions(positions);
}
@Override
public SwerveDriveWheelPositions minus(SwerveDriveWheelPositions other) {
return generate(other, (a, b) -> a.minus(b));
}
@Override
public SwerveDriveWheelPositions interpolate(SwerveDriveWheelPositions endValue, double t) {
return generate(endValue, (a, b) -> a.interpolate(b, t));
if (endValue.positions.length != positions.length) {
throw new IllegalArgumentException("Inconsistent number of modules!");
}
var newPositions = new SwerveModulePosition[positions.length];
for (int i = 0; i < positions.length; i++) {
newPositions[i] = positions[i].interpolate(endValue.positions[i], t);
}
return new SwerveDriveWheelPositions(newPositions);
}
}

13
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
View File

@@ -73,19 +73,6 @@ public class SwerveModulePosition
return new SwerveModulePosition(distanceMeters, angle);
}
/**
* Calculates the difference between two swerve module positions. The difference has a length
* equal to the difference in lengths and an angle equal to the ending angle (this module
* position's angle). This is suitable for representing a module's motion between two timesteps,
* because the final angle describes the direction the module moved.
*
* @param other The swerve module position to subtract.
* @return The difference.
*/
public SwerveModulePosition minus(SwerveModulePosition other) {
return new SwerveModulePosition(this.distanceMeters - other.distanceMeters, this.angle);
}
@Override
public SwerveModulePosition interpolate(SwerveModulePosition endValue, double t) {
return new SwerveModulePosition(

8
wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java
View File

@@ -13,12 +13,4 @@ public interface WheelPositions<T extends WheelPositions<T>> extends Interpolata
* @return A copy.
*/
T copy();
/**
* Returns a representation of how the wheels moved from other to this.
*
* @param other The other instance to compare to.
* @return The representation of how the wheels moved from other to this.
*/
T minus(T other);
}