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

19
wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -49,6 +49,21 @@ ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
Twist2d MecanumDriveKinematics::ToTwist2d(
const MecanumDriveWheelPositions& start,
const MecanumDriveWheelPositions& end) const {
Vectord<4> wheelDeltasVector{
end.frontLeft.value() - start.frontLeft.value(),
end.frontRight.value() - start.frontRight.value(),
end.rearLeft.value() - start.rearLeft.value(),
end.rearRight.value() - start.rearRight.value()};
Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
return {units::meter_t{twistVector(0)}, units::meter_t{twistVector(1)},
units::radian_t{twistVector(2)}};
}
Twist2d MecanumDriveKinematics::ToTwist2d(
const MecanumDriveWheelPositions& wheelDeltas) const {
Vectord<4> wheelDeltasVector{
@@ -57,8 +72,8 @@ Twist2d MecanumDriveKinematics::ToTwist2d(
Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
return {units::meter_t{twistVector(0)}, // NOLINT
units::meter_t{twistVector(1)}, units::radian_t{twistVector(2)}};
return {units::meter_t{twistVector(0)}, units::meter_t{twistVector(1)},
units::radian_t{twistVector(2)}};
}
void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,