# Kinematics

## Inverse Kinematics

Tank drive kinematics are often overlooked in part due to their surface simplicity. Intuitively, for a tank drive, everyone knows that you provide the same voltage to both sides to go forward and opposite voltages to spin. Nevertheless, it's instructive to derive this formally and similar techniques can be applied to mecanum and swerve drives where intuition starts to fail.

Consider the following configuration of a tank robot traveling with constant forward and angular velocity. Throughout this discussion, we will assume that the drive has one wheel per side (this is generally accurate although certain drivetrains may have significant scrub/friction).

![Constant velocity tank path](/files/-Lg_ivPKo5PLkorz0syH)

Here the forward velocity is $$v$$ and the angular velocity is $$\omega$$. The length $$l$$ in the picture is called the track width; it's defined as the distance between the wheels (point of contact is assumed to be in the middle of the wheels). First, the circumference of the circle is $$\Delta x = 2\pi R$$ which implies the robot takes time $$t = \frac{\Delta x}{v} = \frac{2\pi R}{v}$$ to complete a single revolution. Now, we can calculate the wheel velocities: $$v\_l = 2\pi (R - l / 2) \cdot \frac{v}{2\pi R} = v - v \cdot \frac{l}{2R}$$ and $$v\_r = 2\pi (R + l / 2) \cdot \frac{v}{2\pi R} = v + v \cdot \frac{l}{2R}$$. However, we still aren't quite done. In one revolution around the circle, the robot itself also completes one spin relative to the global frame. Thus $$\omega = 2\pi \cdot \frac{v}{2\pi R} = v / R$$. Substituting, we are left with $$v\_l = v - \frac{l\omega}{2}$$ and $$v\_r = v + \frac{l\omega}{2}$$.

Now we have all the ingredients for our trajectory feedforward. The trajectory itself gives the field-centric velocities of the robot. Applying a coordinate frame transform, these velocities are turned into robot-centric velocities. For a tank drive, the robot-centric velocities are $$v$$ and $$\omega$$. The equations derived in the previous paragraph turn $$v$$ and $$\omega$$ into the wheel velocities $$v\_l$$ and $$v\_r$$ which are sent to the motor controller.

## Forward Kinematics and Odometry

As it turns out, the same equations can be used to determine how the robot moved with wheel encoder data. Solving the system for $$v$$ and $$\omega$$ gives (as you should verify) $$v = \frac{v\_r + v\_l}{2}$$ and $$\omega=\frac{v\_r - v\_l}{l}$$. To make the odometry computations tractable, we will split the robot motion into several time segments and assume constant velocity over each segment. The change in position over a given segment is just the velocity multiplied by time. The relationship between the position changes is $$\Delta x = \frac{\Delta x\_r + \Delta x\_l}{2}$$ and $$\Delta \theta = \frac{\Delta x\_r - \Delta x\_l}{l}$$. These position changes are accumulated over time with an initial pose to produce the current pose. In Road Runner, this process is performed incrementally with `Kinematics.relativeOdometryUpdate()`.

### Tracking Wheels

For better pose estimation, some robots have unactuated tracking wheels (sometimes called dead wheels) dedicated to odometry measurements. Three wheels are necessary to compute the full pose. These three wheels are commonly placed in the standard configuration shown below.

![Example standard three-wheel odometry configuration from above. Courtesy of FTC Team 7842.](/files/-M7yKxmAfaKeyoURmqNU)

Road Runner provides built-in localizers for tracking wheels. This configuration is implemented with the following code.

{% tabs %}
{% tab title="Java" %}

```java
public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
    public StandardTrackingWheelLocalizer() {
        super(Arrays.asList(
                new Pose2d(2.6, 7.1, 0.0), // left parallel
                new Pose2d(2.6, -7.1, 0.0), // right parallel
                new Pose2d(-3.75, -0.15, Math.toRadians(90)) // perpendicular
        ));
    }

    @NonNull
    @Override
    public List<Double> getWheelPositions() {
        // return list of encoder readings in inches
    }
}
```

{% endtab %}

{% tab title="Kotlin" %}

```kotlin
class StandardTrackingWheelLocalizer : ThreeTrackingWheelLocalizer(listOf(
    Pose2d(2.6, 7.1, 0.0), // left parallel
    Pose2d(2.6, -7.1, 0.0), // right parallel
    Pose2d(-3.75, -0.15, Math.toRadians(90.0)) // perpendicular
)) {
    override fun getWheelPositions(): List<Double> {
        // return list of encoder readings in inches
    }
}
```

{% endtab %}
{% endtabs %}

You can replace the default localizer of a `Drive` instance with `Drive#setLocalizer()`.


---

# Agent Instructions: Querying This Documentation

If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question.

Perform an HTTP GET request on the current page URL with the `ask` query parameter:

```
GET https://acme-robotics.gitbook.io/road-runner/tour/kinematics.md?ask=<question>
```

The question should be specific, self-contained, and written in natural language.
The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.

Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.