Skip to content

Odometry and Kinematics

This section is a broad overview of the Kinematics and Odometry classes in WPIlib. For a more detailed description, you can check out their section of the wiki here.

What is Odometry?

Odometry is the use of sensor information to estimate the robot's position and rotation on the field. Odometry works for both differential drivetrains and swerve drivetrains (covered in a later section - drivetrains that can move forward, strafe, and rotate, perhaps all three at the same time).

WPILib Odometry Class

Before we cover odometry, we first need to cover the Translation2d, Rotation2d, and Pose2d objects.

Translation2d stores x and y positions of the robot in meters as well as has several other useful methods, including plus(), rotateBy(), times(), and getNorm() (normalization a.k.a the unit vector that points in the same direction as the point).

Rotation2d stores an angular position value in radians as well as has several other useful methods, including getDegrees(), fromDegrees(), rotateBy(), and plus().

Pose2d is a combination of Translation2d and Rotation2d into a singular class, alongside methods such as plus() and relativeTo().

To declare an odometry object, you need to specify the robot's pose and heading. For DifferentialDriveOdometry, you can do this as follows:

Rotation2d heading = /* Get gyro heading */;
// Create odometry at (0, 0) at a specific heading
DifferentialDriveOdometry odometry1 = new DifferentialDriveOdometry(heading)
// Or...
// Create odometry at (x, y) at a specific heading
DifferentialDriveOdometry odometry2 = new DifferentialDriveOdometry(heading, new Pose2d(x, y, heading))

Note that when initializing the odometry with only a heading, the angle in the Pose2d object will not be equal to the heading. The Pose2d angle is relative to its initial heading.

// Create an odometry at (0, 0) facing 90 degrees.
DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(90));
// Print the current rotation. Should be 0 degrees.
System.out.println(odometry.getPoseMeters().getRotation().toDegrees());
// Update the odometry with a new heading
odometry.update(Rotation2d.fromDegrees(225), 0, 0);
// Should be 135 degrees after update
System.out.println(odometry.getPoseMeters().getRotation().toDegrees());

The odometry object has three methods:

  • getPoseMeters() = returns a Pose2d object representing the robot's current position on the field and rotation.

  • resetPosition() = resets the odometry with a new pose and heading.

  • update() = updates the odometry pose based on the robot's heading and speed.

What is Kinematics?

Drivetrain kinematics deals with the relationship between motor speeds and the velocity of the drivetrain. There are two types of kinematic calculations: forward kinematics and inverse kinematics. Forward kinematics calculates the velocity of the drivetrain based on the motor speeds and inverse kinematics calculates the motor speeds based on the velocity of the drivetrain. Like with odometry, there are implementations of kinematics for differential drivetrains and swerve drivetrains.

WPILib Kinematics Class

Before we cover kinematics, we first need to cover the ChassisSpeeds class. ChassisSpeeds is a simple class that contains information about the translational velocity (forward/backward and left/right) and angular velocity (rotation) of the robot. ChassisSpeeds has three primary variables:

  • vxMetersPerSecond = the forward velocity of the drivetrain in m/s. + means moving forward, - means moving backward.

  • vyMetersPerSecond = the lateral velocity of the drivetrain in m/s. + means moving left, - means moving right.

  • omegaRadiansPerSecond = the angular velocity of the drivetrain in rad/s. + means moving counter-clockwise, - means moving clockwise.

Try not to confuse your coordinate systems! X means going forward/backward and Y means going to the left/right.

The Kinematics object can be constructed as follows for a Differential Drivetrain:

// trackWidthMeters is a double representing the distance, in meters, between two opposite wheels on the drivetrain.
DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics‚Äč(trackWidthMeters);

The kinematics class has two useful functions: