WPILibC++ 2024.3.2
MecanumDriveKinematics.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <Eigen/QR>
8#include <wpi/SymbolExports.h>
9
10#include "frc/EigenCore.h"
17#include "wpimath/MathShared.h"
18
19namespace frc {
20
21/**
22 * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
23 * into individual wheel speeds.
24 *
25 * The inverse kinematics (converting from a desired chassis velocity to
26 * individual wheel speeds) uses the relative locations of the wheels with
27 * respect to the center of rotation. The center of rotation for inverse
28 * kinematics is also variable. This means that you can set your set your center
29 * of rotation in a corner of the robot to perform special evasion maneuvers.
30 *
31 * Forward kinematics (converting an array of wheel speeds into the overall
32 * chassis motion) is performs the exact opposite of what inverse kinematics
33 * does. Since this is an overdetermined system (more equations than variables),
34 * we use a least-squares approximation.
35 *
36 * The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
37 * We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
38 * multiply by [wheelSpeeds] to get our chassis speeds.
39 *
40 * Forward kinematics is also used for odometry -- determining the position of
41 * the robot on the field using encoders and a gyro.
42 */
44 : public Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
45 public:
46 /**
47 * Constructs a mecanum drive kinematics object.
48 *
49 * @param frontLeftWheel The location of the front-left wheel relative to the
50 * physical center of the robot.
51 * @param frontRightWheel The location of the front-right wheel relative to
52 * the physical center of the robot.
53 * @param rearLeftWheel The location of the rear-left wheel relative to the
54 * physical center of the robot.
55 * @param rearRightWheel The location of the rear-right wheel relative to the
56 * physical center of the robot.
57 */
58 explicit MecanumDriveKinematics(Translation2d frontLeftWheel,
59 Translation2d frontRightWheel,
60 Translation2d rearLeftWheel,
61 Translation2d rearRightWheel)
62 : m_frontLeftWheel{frontLeftWheel},
63 m_frontRightWheel{frontRightWheel},
64 m_rearLeftWheel{rearLeftWheel},
65 m_rearRightWheel{rearRightWheel} {
66 SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel,
67 rearRightWheel);
68 m_forwardKinematics = m_inverseKinematics.householderQr();
71 }
72
74
75 /**
76 * Performs inverse kinematics to return the wheel speeds from a desired
77 * chassis velocity. This method is often used to convert joystick values into
78 * wheel speeds.
79 *
80 * This function also supports variable centers of rotation. During normal
81 * operations, the center of rotation is usually the same as the physical
82 * center of the robot; therefore, the argument is defaulted to that use case.
83 * However, if you wish to change the center of rotation for evasive
84 * maneuvers, vision alignment, or for any other use case, you can do so.
85 *
86 * @param chassisSpeeds The desired chassis speed.
87 * @param centerOfRotation The center of rotation. For example, if you set the
88 * center of rotation at one corner of the robot and
89 * provide a chassis speed that only has a dtheta
90 * component, the robot will rotate around that
91 * corner.
92 *
93 * @return The wheel speeds. Use caution because they are not normalized.
94 * Sometimes, a user input may cause one of the wheel speeds to go
95 * above the attainable max velocity. Use the
96 * MecanumDriveWheelSpeeds::Normalize() function to rectify this
97 * issue. In addition, you can leverage the power of C++17 to directly
98 * assign the wheel speeds to variables:
99 *
100 * @code{.cpp}
101 * auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(chassisSpeeds);
102 * @endcode
103 */
105 const ChassisSpeeds& chassisSpeeds,
106 const Translation2d& centerOfRotation) const;
107
109 const ChassisSpeeds& chassisSpeeds) const override {
111 }
112
113 /**
114 * Performs forward kinematics to return the resulting chassis state from the
115 * given wheel speeds. This method is often used for odometry -- determining
116 * the robot's position on the field using data from the real-world speed of
117 * each wheel on the robot.
118 *
119 * @param wheelSpeeds The current mecanum drive wheel speeds.
120 *
121 * @return The resulting chassis speed.
122 */
124 const MecanumDriveWheelSpeeds& wheelSpeeds) const override;
125
127 const MecanumDriveWheelPositions& end) const override;
128
129 /**
130 * Performs forward kinematics to return the resulting Twist2d from the
131 * given wheel position deltas. This method is often used for odometry --
132 * determining the robot's position on the field using data from the
133 * distance driven by each wheel on the robot.
134 *
135 * @param wheelDeltas The change in distance driven by each wheel.
136 *
137 * @return The resulting chassis speed.
138 */
140
141 /**
142 * Returns the front-left wheel translation.
143 *
144 * @return The front-left wheel translation.
145 */
146 const Translation2d& GetFrontLeft() const { return m_frontLeftWheel; }
147
148 /**
149 * Returns the front-right wheel translation.
150 *
151 * @return The front-right wheel translation.
152 */
153 const Translation2d& GetFrontRight() const { return m_frontRightWheel; }
154
155 /**
156 * Returns the rear-left wheel translation.
157 *
158 * @return The rear-left wheel translation.
159 */
160 const Translation2d& GetRearLeft() const { return m_rearLeftWheel; }
161
162 /**
163 * Returns the rear-right wheel translation.
164 *
165 * @return The rear-right wheel translation.
166 */
167 const Translation2d& GetRearRight() const { return m_rearRightWheel; }
168
169 private:
170 mutable Matrixd<4, 3> m_inverseKinematics;
171 Eigen::HouseholderQR<Matrixd<4, 3>> m_forwardKinematics;
172 Translation2d m_frontLeftWheel;
173 Translation2d m_frontRightWheel;
174 Translation2d m_rearLeftWheel;
175 Translation2d m_rearRightWheel;
176
177 mutable Translation2d m_previousCoR;
178
179 /**
180 * Construct inverse kinematics matrix from wheel locations.
181 *
182 * @param fl The location of the front-left wheel relative to the physical
183 * center of the robot.
184 * @param fr The location of the front-right wheel relative to the physical
185 * center of the robot.
186 * @param rl The location of the rear-left wheel relative to the physical
187 * center of the robot.
188 * @param rr The location of the rear-right wheel relative to the physical
189 * center of the robot.
190 */
191 void SetInverseKinematics(Translation2d fl, Translation2d fr,
192 Translation2d rl, Translation2d rr) const;
193};
194
195} // namespace frc
196
197
198
#define WPILIB_DLLEXPORT
Definition: SymbolExports.h:36
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel s...
Definition: Kinematics.h:23
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel s...
Definition: MecanumDriveKinematics.h:44
MecanumDriveWheelSpeeds ToWheelSpeeds(const ChassisSpeeds &chassisSpeeds, const Translation2d &centerOfRotation) const
Performs inverse kinematics to return the wheel speeds from a desired chassis velocity.
Twist2d ToTwist2d(const MecanumDriveWheelPositions &wheelDeltas) const
Performs forward kinematics to return the resulting Twist2d from the given wheel position deltas.
const Translation2d & GetRearRight() const
Returns the rear-right wheel translation.
Definition: MecanumDriveKinematics.h:167
const Translation2d & GetFrontRight() const
Returns the front-right wheel translation.
Definition: MecanumDriveKinematics.h:153
Twist2d ToTwist2d(const MecanumDriveWheelPositions &start, const MecanumDriveWheelPositions &end) const override
Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions.
MecanumDriveKinematics(const MecanumDriveKinematics &)=default
MecanumDriveKinematics(Translation2d frontLeftWheel, Translation2d frontRightWheel, Translation2d rearLeftWheel, Translation2d rearRightWheel)
Constructs a mecanum drive kinematics object.
Definition: MecanumDriveKinematics.h:58
const Translation2d & GetFrontLeft() const
Returns the front-left wheel translation.
Definition: MecanumDriveKinematics.h:146
const Translation2d & GetRearLeft() const
Returns the rear-left wheel translation.
Definition: MecanumDriveKinematics.h:160
MecanumDriveWheelSpeeds ToWheelSpeeds(const ChassisSpeeds &chassisSpeeds) const override
Performs inverse kinematics to return the wheel speeds from a desired chassis velocity.
Definition: MecanumDriveKinematics.h:108
ChassisSpeeds ToChassisSpeeds(const MecanumDriveWheelSpeeds &wheelSpeeds) const override
Performs forward kinematics to return the resulting chassis state from the given wheel speeds.
Represents a translation in 2D space.
Definition: Translation2d.h:27
static void ReportUsage(MathUsageId id, int count)
Definition: MathShared.h:73
Definition: AprilTagPoseEstimator.h:15
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition: EigenCore.h:21
Represents the speed of a robot chassis.
Definition: ChassisSpeeds.h:25
Represents the wheel positions for a mecanum drive drivetrain.
Definition: MecanumDriveWheelPositions.h:16
Represents the wheel speeds for a mecanum drive drivetrain.
Definition: MecanumDriveWheelSpeeds.h:15
A change in distance along a 2D arc since the last pose update.
Definition: Twist2d.h:21