Mecanum Drive Kinematics

import com.arcrobotics.ftclib.kinematics.wpilibkinematics.MecanumDriveKinematics

The MecanumDriveKinematics class is a useful tool that converts between a ChassisSpeeds object and a MecanumDriveWheelSpeeds object, which contains velocities for each of the four wheels on a mecanum drive.

Constructing the Kinematics Object

The MecanumDriveKinematics class accepts four constructor arguments, with each argument being the location of a wheel relative to the robot center (as a Translation2d). The order for the arguments is front left, front right, back left, and back right. The locations for the wheels must be relative to the center of the robot. Positive x values represent moving toward the front of the robot whereas positive y values represent moving toward the left of the robot.

// Locations of the wheels relative to the robot center.
Translation2d m_frontLeftLocation =
  new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation =
  new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation =
  new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation =
  new Translation2d(-0.381, -0.381);

// Creating my kinematics object using the wheel locations.
MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics
(
  m_frontLeftLocation, m_frontRightLocation,
  m_backLeftLocation, m_backRightLocation
);

Converting Chassis Speeds to Wheel Speeds

The toWheelSpeeds(ChassisSpeeds speeds) method should be used to convert a ChassisSpeeds object to a MecanumDriveWheelSpeeds object. This is useful in situations where you have to convert a forward velocity, sideways velocity, and an angular velocity into individual wheel speeds.

// Example chassis speeds: 1 meter per second forward, 3 meters
// per second to the left, and rotation at 1.5 radians per second
// counterclockwise.
ChassisSpeeds speeds = new ChassisSpeeds(1.0, 3.0, 1.5);

// Convert to wheel speeds
MecanumDriveWheelSpeeds wheelSpeeds =
    kinematics.toWheelSpeeds(speeds);

// Get the individual wheel speeds
double frontLeft = wheelSpeeds.frontLeftMetersPerSecond;
double frontRight = wheelSpeeds.frontRightMetersPerSecond;
double backLeft = wheelSpeeds.rearLeftMetersPerSecond;
double backRight = wheelSpeeds.rearRightMetersPerSecond;

Field-Oriented Drive

Recall that a ChassisSpeeds object can be created from a set of desired field-oriented speeds. This feature can be used to get wheel speeds from a set of desired field-oriented speeds.

// The desired field relative speed here is 2 meters per second
// toward the opponent's alliance station wall, and 2 meters per
// second toward the left field boundary. The desired rotation
// is a quarter of a rotation per second counterclockwise.
// The current robot angle is 45 degrees.
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
  2.0, 2.0, Math.PI / 2.0, Rotation2d.fromDegrees(45.0)
);

// Now use this in our kinematics
MecanumDriveWheelSpeeds wheelSpeeds =
  kinematics.toWheelSpeeds(speeds);

Using Custom Centers of Rotation

Sometimes, rotating around one specific corner might be desirable for certain evasive maneuvers. This type of behavior is also supported by the WPILib classes. The same toWheelSpeeds() method accepts a second parameter for the center of rotation (as a Translation2d). Just like the wheel locations, the Translation2d representing the center of rotation should be relative to the robot center.

Because all robots are a rigid frame, the provided vx and vy velocities from the ChassisSpeeds object will still apply for the entirety of the robot. However, the omega from the ChassisSpeeds object will be measured from the center of rotation.

For example, one can set the center of rotation on a certain wheel and if the provided ChassisSpeeds object has a vx and vy of zero and a non-zero omega, the robot will appear to rotate around that particular wheel.

Converting Wheel Speeds to Chassis speeds

One can also use the kinematics object to convert a MecanumDriveWheelSpeeds object to a singular ChassisSpeeds object. The toChassisSpeeds(MecanumDriveWheelSpeeds speeds) method can be used to achieve this.

// Example wheel speeds
MecanumDriveWheelSpeeds wheelSpeeds =
    new MecanumDriveWheelSpeeds(-17.67, 20.51, -13.44, 16.26);

// Convert to chassis speeds
ChassisSpeeds chassisSpeeds =
    kinematics.toChassisSpeeds(wheelSpeeds);

// Getting individual speeds
double forward = chassisSpeeds.vxMetersPerSecond;
double sideways = chassisSpeeds.vyMetersPerSecond;
double angular = chassisSpeeds.omegaRadiansPerSecond;