mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
88 lines
3.6 KiB
C++
88 lines
3.6 KiB
C++
// 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.
|
|
|
|
#include "frc/kinematics/MecanumDriveKinematics.h"
|
|
|
|
using namespace frc;
|
|
|
|
MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
|
|
const ChassisSpeeds& chassisSpeeds,
|
|
const Translation2d& centerOfRotation) const {
|
|
// We have a new center of rotation. We need to compute the matrix again.
|
|
if (centerOfRotation != m_previousCoR) {
|
|
auto fl = m_frontLeftWheel - centerOfRotation;
|
|
auto fr = m_frontRightWheel - centerOfRotation;
|
|
auto rl = m_rearLeftWheel - centerOfRotation;
|
|
auto rr = m_rearRightWheel - centerOfRotation;
|
|
|
|
SetInverseKinematics(fl, fr, rl, rr);
|
|
|
|
m_previousCoR = centerOfRotation;
|
|
}
|
|
|
|
Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
|
|
chassisSpeeds.vy.value(),
|
|
chassisSpeeds.omega.value()};
|
|
|
|
Eigen::Vector4d wheelsVector = m_inverseKinematics * chassisSpeedsVector;
|
|
|
|
MecanumDriveWheelSpeeds wheelSpeeds;
|
|
wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
|
|
wheelSpeeds.frontRight = units::meters_per_second_t{wheelsVector(1)};
|
|
wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsVector(2)};
|
|
wheelSpeeds.rearRight = units::meters_per_second_t{wheelsVector(3)};
|
|
return wheelSpeeds;
|
|
}
|
|
|
|
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
|
|
const MecanumDriveWheelSpeeds& wheelSpeeds) const {
|
|
Eigen::Vector4d wheelSpeedsVector{
|
|
wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
|
|
wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
|
|
|
|
Eigen::Vector3d chassisSpeedsVector =
|
|
m_forwardKinematics.solve(wheelSpeedsVector);
|
|
|
|
return {units::meters_per_second_t{chassisSpeedsVector(0)}, // NOLINT
|
|
units::meters_per_second_t{chassisSpeedsVector(1)},
|
|
units::radians_per_second_t{chassisSpeedsVector(2)}};
|
|
}
|
|
|
|
Twist2d MecanumDriveKinematics::ToTwist2d(
|
|
const MecanumDriveWheelPositions& start,
|
|
const MecanumDriveWheelPositions& end) const {
|
|
Eigen::Vector4d 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 {
|
|
Eigen::Vector4d wheelDeltasVector{
|
|
wheelDeltas.frontLeft.value(), wheelDeltas.frontRight.value(),
|
|
wheelDeltas.rearLeft.value(), wheelDeltas.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)}};
|
|
}
|
|
|
|
void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
|
|
Translation2d fr,
|
|
Translation2d rl,
|
|
Translation2d rr) const {
|
|
m_inverseKinematics = Matrixd<4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()},
|
|
{1, 1, (fr.X() - fr.Y()).value()},
|
|
{1, 1, (rl.X() - rl.Y()).value()},
|
|
{1, -1, (-(rr.X() + rr.Y())).value()}};
|
|
}
|