/
MecanumDriveKinematics.cpp
69 lines (57 loc) · 3.05 KB
/
MecanumDriveKinematics.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/MecanumDriveKinematics.h"
using namespace frc;
MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
// We have a new center of rotation. We need to compute the matrix again.
if (centerOfRotation.X() != m_previousCoR.X() ||
centerOfRotation.Y() != m_previousCoR.Y()) {
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;
chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
Eigen::Matrix<double, 4, 1> wheelsMatrix =
m_inverseKinematics * chassisSpeedsVector;
MecanumDriveWheelSpeeds wheelSpeeds;
wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)};
wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)};
wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)};
wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)};
return wheelSpeeds;
}
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) {
Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
// clang-format off
wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>();
// clang-format on
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(wheelSpeedsMatrix);
return {units::meters_per_second_t{chassisSpeedsVector(0)},
units::meters_per_second_t{chassisSpeedsVector(1)},
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
Translation2d fr,
Translation2d rl,
Translation2d rr) {
// clang-format off
m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
1, 1, (fr.X() - fr.Y()).template to<double>(),
1, 1, (rl.X() - rl.Y()).template to<double>(),
1, -1, (-(rr.X() + rr.Y())).template to<double>();
// clang-format on
m_inverseKinematics /= std::sqrt(2);
}