Class MecanumDriveKinematics

java.lang.Object

org.wpilib.math.kinematics.MecanumDriveKinematics

All Implemented Interfaces:

Interpolator<MecanumDriveWheelPositions>, Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelVelocities, MecanumDriveWheelAccelerations>, ProtobufSerializable, StructSerializable, WPISerializable

public class MecanumDriveKinematics
extends Object
implements Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelVelocities, MecanumDriveWheelAccelerations>, ProtobufSerializable, StructSerializable

Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel velocities.

The inverse kinematics (converting from a desired chassis velocity to individual wheel velocities) uses the relative locations of the wheels with respect to the center of rotation. The center of rotation for inverse kinematics is also variable. This means that you can set your center of rotation in a corner of the robot to perform special evasion maneuvers.

Forward kinematics (converting an array of wheel velocities into the overall chassis motion) is performs the exact opposite of what inverse kinematics does. Since this is an overdetermined system (more equations than variables), we use a least-squares approximation.

The inverse kinematics: [wheelVelocities] = [wheelLocations] * [chassisVelocities] We take the Moore-Penrose pseudoinverse of [wheelLocations] and then multiply by [wheelVelocities] to get our chassis velocities.

Forward kinematics is also used for odometry -- determining the position of the robot on the field using encoders and a gyro.

Field Summary

Fields

Modifier and Type	Field	Description
static final MecanumDriveKinematicsProto	proto	MecanumDriveKinematics protobuf for serialization.
static final MecanumDriveKinematicsStruct	struct	MecanumDriveKinematics struct for serialization.

Constructor Summary

Constructors

Constructor	Description
MecanumDriveKinematics(Translation2d frontLeftWheel, Translation2d frontRightWheel, Translation2d rearLeftWheel, Translation2d rearRightWheel)	Constructs a mecanum drive kinematics object.

Method Summary

Modifier and Type	Method	Description
MecanumDriveWheelPositions	copy(MecanumDriveWheelPositions positions)	Returns a copy of the wheel positions object.
void	copyInto(MecanumDriveWheelPositions positions, MecanumDriveWheelPositions output)	Copies the value of the wheel positions object into the output.
Translation2d	getFrontLeft()	Returns the front-left wheel translation.
Translation2d	getFrontRight()	Returns the front-right wheel translation.
Translation2d	getRearLeft()	Returns the rear-left wheel translation.
Translation2d	getRearRight()	Returns the rear-right wheel translation.
MecanumDriveWheelPositions	interpolate(MecanumDriveWheelPositions startValue, MecanumDriveWheelPositions endValue, double t)	Perform interpolation between two values.
ChassisAccelerations	toChassisAccelerations(MecanumDriveWheelAccelerations wheelAccelerations)	Performs forward kinematics to return the resulting chassis accelerations from the given wheel accelerations.
ChassisVelocities	toChassisVelocities(MecanumDriveWheelVelocities wheelVelocities)	Performs forward kinematics to return the resulting chassis state from the given wheel velocities.
Twist2d	toTwist2d(MecanumDriveWheelPositions wheelDeltas)	Performs forward kinematics to return the resulting Twist2d from the given wheel deltas.
Twist2d	toTwist2d(MecanumDriveWheelPositions start, MecanumDriveWheelPositions end)	Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions.
MecanumDriveWheelAccelerations	toWheelAccelerations(ChassisAccelerations chassisAccelerations)	Performs inverse kinematics.
MecanumDriveWheelAccelerations	toWheelAccelerations(ChassisAccelerations chassisAccelerations, Translation2d centerOfRotation)	Performs inverse kinematics to return the wheel accelerations from a desired chassis acceleration.
MecanumDriveWheelVelocities	toWheelVelocities(ChassisVelocities chassisVelocities)	Performs inverse kinematics.
MecanumDriveWheelVelocities	toWheelVelocities(ChassisVelocities chassisVelocities, Translation2d centerOfRotation)	Performs inverse kinematics to return the wheel velocities from a desired chassis velocity.

Methods inherited from class Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Details

proto

public static final MecanumDriveKinematicsProto proto

MecanumDriveKinematics protobuf for serialization.

struct

public static final MecanumDriveKinematicsStruct struct

MecanumDriveKinematics struct for serialization.

Constructor Details

MecanumDriveKinematics

public MecanumDriveKinematics(Translation2d frontLeftWheel,
 Translation2d frontRightWheel,
 Translation2d rearLeftWheel,
 Translation2d rearRightWheel)

Constructs a mecanum drive kinematics object.

Parameters:

frontLeftWheel - The location of the front-left wheel relative to the physical center of the robot, in meters.

frontRightWheel - The location of the front-right wheel relative to the physical center of the robot, in meters.

rearLeftWheel - The location of the rear-left wheel relative to the physical center of the robot, in meters.

rearRightWheel - The location of the rear-right wheel relative to the physical center of the robot, in meters.

Method Details

toWheelVelocities

public MecanumDriveWheelVelocities toWheelVelocities(ChassisVelocities chassisVelocities,
 Translation2d centerOfRotation)

Performs inverse kinematics to return the wheel velocities from a desired chassis velocity. This method is often used to convert joystick values into wheel velocities.

This function also supports variable centers of rotation. During normal operations, the center of rotation is usually the same as the physical center of the robot; therefore, the argument is defaulted to that use case. However, if you wish to change the center of rotation for evasive maneuvers, vision alignment, or for any other use case, you can do so.

Parameters:

chassisVelocities - The desired chassis velocity.

centerOfRotation - The center of rotation. For example, if you set the center of rotation at one corner of the robot and provide a chassis velocity that only has a dtheta component, the robot will rotate around that corner.

Returns:

The wheel velocities. Use caution because they are not normalized. Sometimes, a user input may cause one of the wheel velocities to go above the attainable max velocity. Use the MecanumDriveWheelVelocities.desaturate(double) function to rectify this issue.

toWheelVelocities

public MecanumDriveWheelVelocities toWheelVelocities(ChassisVelocities chassisVelocities)

Performs inverse kinematics. See toWheelVelocities(ChassisVelocities, Translation2d) for more information.

Specified by:

toWheelVelocities in interface Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelVelocities, MecanumDriveWheelAccelerations>

Parameters:

chassisVelocities - The desired chassis velocity.

Returns:

The wheel velocities.

toChassisVelocities

public ChassisVelocities toChassisVelocities(MecanumDriveWheelVelocities wheelVelocities)

Performs forward kinematics to return the resulting chassis state from the given wheel velocities. This method is often used for odometry -- determining the robot's position on the field using data from the real-world velocity of each wheel on the robot.

Specified by:

toChassisVelocities in interface Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelVelocities, MecanumDriveWheelAccelerations>

Parameters:

wheelVelocities - The current mecanum drive wheel velocities.

Returns:

The resulting chassis velocity.

toWheelAccelerations

public MecanumDriveWheelAccelerations toWheelAccelerations(ChassisAccelerations chassisAccelerations,
 Translation2d centerOfRotation)

Performs inverse kinematics to return the wheel accelerations from a desired chassis acceleration. This method is often used for dynamics calculations -- converting desired robot accelerations into individual wheel accelerations.

This function also supports variable centers of rotation. During normal operations, the center of rotation is usually the same as the physical center of the robot; therefore, the argument is defaulted to that use case. However, if you wish to change the center of rotation for evasive maneuvers, vision alignment, or for any other use case, you can do so.

Parameters:

chassisAccelerations - The desired chassis accelerations.

centerOfRotation - The center of rotation. For example, if you set the center of rotation at one corner of the robot and provide a chassis acceleration that only has a dtheta component, the robot will rotate around that corner.

Returns:

The wheel accelerations.

toWheelAccelerations

public MecanumDriveWheelAccelerations toWheelAccelerations(ChassisAccelerations chassisAccelerations)

Performs inverse kinematics. See toWheelAccelerations(ChassisAccelerations, Translation2d) for more information.

Specified by:

toWheelAccelerations in interface Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelVelocities, MecanumDriveWheelAccelerations>

Parameters:

chassisAccelerations - The desired chassis accelerations.

Returns:

The wheel accelerations.

toChassisAccelerations

public ChassisAccelerations toChassisAccelerations(MecanumDriveWheelAccelerations wheelAccelerations)

Performs forward kinematics to return the resulting chassis accelerations from the given wheel accelerations. This method is often used for dynamics calculations -- determining the robot's acceleration on the field using data from the real-world acceleration of each wheel on the robot.

Specified by:

toChassisAccelerations in interface Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelVelocities, MecanumDriveWheelAccelerations>

Parameters:

wheelAccelerations - The current mecanum drive wheel accelerations.

Returns:

The resulting chassis accelerations.

toTwist2d

public Twist2d toTwist2d(MecanumDriveWheelPositions start,
 MecanumDriveWheelPositions end)

Description copied from interface: Kinematics

Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions. This method is often used for odometry -- determining the robot's position on the field using changes in the distance driven by each wheel on the robot.

Specified by:

toTwist2d in interface Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelVelocities, MecanumDriveWheelAccelerations>

Parameters:

start - The starting distances driven by the wheels.

end - The ending distances driven by the wheels.

Returns:

The resulting Twist2d in the robot's movement.

toTwist2d

public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas)

Performs forward kinematics to return the resulting Twist2d from the given wheel deltas. This method is often used for odometry -- determining the robot's position on the field using changes in the distance driven by each wheel on the robot.

Parameters:

wheelDeltas - The distances driven by each wheel.

Returns:

The resulting Twist2d.

getFrontLeft

public Translation2d getFrontLeft()

Returns the front-left wheel translation.

Returns:

The front-left wheel translation.

getFrontRight

public Translation2d getFrontRight()

Returns the front-right wheel translation.

Returns:

The front-right wheel translation.

getRearLeft

public Translation2d getRearLeft()

Returns the rear-left wheel translation.

Returns:

The rear-left wheel translation.

getRearRight

public Translation2d getRearRight()

Returns the rear-right wheel translation.

Returns:

The rear-right wheel translation.

copy

public MecanumDriveWheelPositions copy(MecanumDriveWheelPositions positions)

Description copied from interface: Kinematics

Returns a copy of the wheel positions object.

Specified by:

copy in interface Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelVelocities, MecanumDriveWheelAccelerations>

Parameters:

positions - The wheel positions object to copy.

Returns:

A copy.

copyInto

public void copyInto(MecanumDriveWheelPositions positions,
 MecanumDriveWheelPositions output)

Description copied from interface: Kinematics

Copies the value of the wheel positions object into the output.

Specified by:

copyInto in interface Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelVelocities, MecanumDriveWheelAccelerations>

Parameters:

positions - The wheel positions object to copy. Will not be modified.

output - The output variable of the copy operation. Will have the same value as the positions object after the call.

interpolate

public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions startValue,
 MecanumDriveWheelPositions endValue,
 double t)

Description copied from interface: Interpolator

Perform interpolation between two values.

Specified by:

interpolate in interface Interpolator<MecanumDriveWheelPositions>

Parameters:

startValue - The value to start at.

endValue - The value to end at.

t - How far between the two values to interpolate. This should be bounded to [0, 1].

Returns:

The interpolated value.