Mechanical Design

• Motors & Controllers
• Drive
• Azimuth - turns

Put a picture here.

Swerve Operation

• Forward / Back
• Strafe Left / Right

Swerve Control Systems

• Azimuth closed-loop
• Drive open-loop
• Drive closed-loop

Swerve Motion

There are three components to swerve drive motion.

• Forward and backwards
• Strafe left and right
• Yaw counter-clockwise and clockwise

The red arrows indicate the speed and direction each wheel is pushing.

We call this the velocity vector of the wheel.

Swerve Motion

These separate components can be combined together to move the robot in any direction.

In this example, equal amounts of forward and strafe input drive the robot at 45° across the field.

Swerve Motion

All three components can be combined together to produce complex motion.

In this example, equal amounts of forward, strafe and yaw input drive the robot across the field while yawing.

Swerve Yaw Component

When the robot yaws, the wheels are positioned perpendicular to the center of rotation.

Note that this is not 45° if the robot isn’t square.

We can also yaw around an arbitrary off-center point relative to the robot.

Vectors

We use vectors to represent velocity, which is speed and direction.

Here we show how to add two vectors together, later we will show rotating a vector by a given angle.

Coordinate System

We work in two coordinate frames, one local to the robot and one global for the field.

Going forward, we’ll also refer to swerve drive motion components (forward, strafe, yaw) as chassis speed (v_x, v_y, and ω), with respect to the robot frame of reference.

Wheel Position and Speed

We add together the desired robot-oriented velocity inputs to get a velocity vector for each wheel.

Wheel Speed and Azimuth

Each wheel’s speed and azimuth can be calculated from its velocity vector (v_w).

Azimuth (θ) is the term we use for the wheel’s angle relative to the robot frame of reference.

Normalize Wheel Speeds

Sometimes after calculating wheel velocity vectors, the requested speed may be above the maximum attainable speed for the drive motor on that swerve module.

To fix this issue, we “normalize” all the wheel speeds to make sure that all requested module speeds are below the absolute threshold, while maintaining the ratio of speeds between modules.

Optimize Wheel Position

We minimize the change in heading the desired swerve wheel direction would require by potentially reversing the direction the wheel spins.

When optimized, the furthest a wheel will ever rotate is 90 degrees.

Field Oriented Driving

In Teleoperation, we normally provide driver joystick (velocity) inputs relative to the field frame of reference.

We rotate the requested field-oriented robot velocity vector by an amount equal to the gyro angle, θ, to get the desired robot-oriented velocity vector.

In this example, pushing the v_x (F/R) joystick in the postive x direction will move the robot directly down the field, no matter the direction the robot is facing (θ).

Terminology

Kinematics

The kinematics classes help convert between a universal ChassisSpeeds object, containing linear and angular velocities for a robot to usable speeds for each individual module states (speed and angle) for a swerve drive.

Odometry

Odometry uses sensors on the robot to create an estimate of the position of the robot on the field. In our robots, these sensors are typically drive and azimuth encoders and a gyroscope to measure robot angle.

The odometry classes utilize the kinematics classes along with periodic inputs about speeds and angles to create an estimate of the robot’s location on the field.

Pose

A robot’s pose is the combination of it’s X, Y, and angle (θ) with respect to the field.

the ChassisSpeeds Class

The ChassisSpeeds class represents the speeds of a robot chassis.

vx

The velocity of the robot in the x (forward) direction.

vy

The velocity of the robot in the y (strafe) direction.
Positive values mean the robot is moving to the left.

omega

The angular velocity of the robot in radians per second.
Positive is CCW rotation (yaw) of the robot.

double xSpeed = leftJoystick.x * kMaxMetersPerSec;
double ySpeed = leftJoystick.y * kMaxMetersPerSec;
double rotSpeed = rightJoystick.x * kMaxRadiansPerSec;

ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rotSpeed);

Speeds are aligned to a coordinate system and are given in meters per second.

We also use the ChassisSpeeds to convert field-relative speeds into robot-relative speeds.

double xSpeed = leftJoystick.x * kMaxMetersPerSec;
double ySpeed = leftJoystick.y * kMaxMetersPerSec;
double rotSpeed = rightJoystick.x * kMaxRadiansPerSec;

ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotSpeed, gyro.getRotation2d());

The angle of the robot is measured by a gyroscope. The robot’s angle is considered to be zero when it is facing directly away from our alliance station wall.

The SwerveDriveKinematics Class

The SwerveDriveKinematics class is a helper class that converts a chassis velocity (v_x, v_y, and ω components) into individual module states (speed and angle).

We initialize an instance of this class by passing in four wheel locations, in meters, relative to the center of the robot.

var frontLeft = new Translation2d(0.3, 0.3);
var frontRight = new Translation2d(0.3, -0.3);
var rearLeft = new Translation2d(-0.3, 0.3);
var rearRight = new Translation2d(-0.3, -0.3);

var kinematics = new SwerveDriveKinematics(frontLeft, frontRight, rearLeft, rearRight);

We then use this kinematics instance to perform inverse kinematics to return the module states from a desired chassis velocity.

var speeds = new ChassisSpeeds(xSpeed, ySpeed, rotSpeed);
SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);

The returned module states are an array of four SwerveModuleState objects, each containing the speed and angle of one of the wheels. They are passed back in the same wheel order that we initialized the SwerveDriveKinematics in.

If you want to specify a variable center of rotation for the robot, you can pass in a optional Translation2d object that is the desired center.

For example, you want to yaw the robot underneath the shooter that is 10 cm left of the centerline of the robot.

var speeds = new ChassisSpeeds(0, 0, rotSpeed);
var center = new Translation2d(0, 0.1);
SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds, center);

Sometimes after calculating wheel velocity vectors, the requested speed may be above the maximum attainable speed for the swerve module drive motor and need to be normalized.

To fix this issue, SwerveDriveKinematics has a normalizeWheelSpeeds​ static method.

SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds, center);
SwerveDriveKinematics.normalizeWheelSpeeds(states, kMaxMetersPerSec);
// all state speeds are now less than or equal to kMaxMetersPerSec...

We can also use this kinematics instance to perform foward kinematics to return the instantaneous chassis velocity from module states.

Typically we would query the hardware encoders to get actual module states, as in this example.

// driveSubsystem has code to read swerve module hardware encoders and converts to module states
SwerveModuleState[] states = driveSubsystem.getModuleStates();

ChassisSpeeds speeds = kinematics.toChassisSpeeds(states);

System.out.printf(
    "robot vxMetersPerSecond = %f, vyMetersPerSecond = %f, omegaRadiansPerSecond = %f %n",
    speeds.vxMetersPerSecond,
    speeds.vyMetersPerSecond,
    speeds.omegaRadiansPerSecond
);

The SwerveModuleState Class

The SwerveModuleState is a simple data class that represents the speed and direction of a swerve module.

We would typically pass it to our own SwerveModule class that knows about our specific hardware.

SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds, center);
 SwerveDriveKinematics.normalizeWheelSpeeds(states, kMaxMetersPerSec);
frontLeftModule.setDesiredState(states[0]);
// rest of modules...

It also performs the optimization of wheel positioning by minimizing the change in heading the desired swerve wheel direction would require by potentially reversing the direction the wheel spins. For example, our SwerveModule class could use it in its setDesiredState method.

public void setDesiredState(SwerveModuleState desiredState) {
    SwerveModuleState state = SwerveModuleState.optimize(desiredState, new Rotation2d(azimuth.getEncoderValue()));
    // use optimized state to set module speed and angle...
}

The SwerveDriveOdometry Class

Odometry allows us to track our robot’s position on the field over a course of a match using readings from swerve drive encoders and swerve azimuth encoders.

We initialize an instance of this class by passing in our initialized SwerveDriveKinimatics object, the robot’s gyro angle, and optionally a starting robot pose.

var kinematics = new SwerveDriveKinematics(frontLeft, frontRight, rearLeft, rearRight);
var theta = gyro.getRotation2d();

var odometry = new SwerveDriveOdometry(kinematics, theta);

Periodically (for example, in Subsystem.periodic()), we update the odometry with the current gyro angle and swerve module states.

void periodic() {
    var theta = gyro.getRotation2d();
    SwerveModuleState[] states = getModuleStates();
    odometry.update(theta, states);
}

We can perform a “hard” reset of the odometry position if we are in possession of a known robot position. For example, perhaps we have just driven the robot into some sort of game feature like a docking port.

Pose2d pose = Constants.DOCKING_POSE;
var gyroAngle = gyro.getRotation2d();
odometry.resetPosition(pose, gyroAngle);

By passing in the gyro angle, odometry will compensate for any gyro drift that has occurred up to this point.

Finally, we can ask odometry for the robot’s calculated field position and angle. This returns the pose of the robot as of the last call to update().

Pose2d pose = odometry.getPoseMeters();
System.out.printf(
    "robot x = %f meters, y = %f meters, theta = %f degrees %n",
    pose.getX(),
    pose.getY(),
    pose.getRotation().getDegrees()
);

We can use odometry during the autonomous period for complex tasks like path following. Furthermore, odometry can be used for latency compensation when using computer-vision systems.

Swerve Kinematics Examples

We can use the Python version of WPILIB kinematics classes in a Jupyter notebook to easily convert desired robot speed and rotation into swerve wheel speeds and angles.

The classes and concepts are identical to the Java version we use on the robot.

# define some useful constants
MAX_SPEED = 1
MAX_ROTATION = 0.5 * math.pi

# set up the swerve drive kinematics class by specifying where the wheels are
# relative to the center of the robot
fl_loc = Translation2d(0.3, 0.3)
fr_loc = Translation2d(0.3, -0.3)
rl_loc = Translation2d(-0.3, 0.3)
rr_loc = Translation2d(-0.3, -0.3)

kinematics = SwerveDrive4Kinematics(fl_loc, fr_loc, rl_loc, rr_loc)

Drive in forward direction

speeds = ChassisSpeeds(MAX_SPEED, 0, 0)
module_states = kinematics.toSwerveModuleStates(speeds)
module_states = kinematics.normalizeWheelSpeeds(module_states, MAX_SPEED)
plot_swerve(wheel_locs, module_states)

Drive in forward, right directions

speeds = ChassisSpeeds(MAX_SPEED, -MAX_SPEED, 0)
module_states = kinematics.toSwerveModuleStates(speeds)
module_states = kinematics.normalizeWheelSpeeds(module_states, MAX_SPEED)
plot_swerve(wheel_locs, module_states)

Drive in forward, right and clockwise directions

speeds = ChassisSpeeds(MAX_SPEED, -MAX_SPEED, -MAX_ROTATION)
module_states = kinematics.toSwerveModuleStates(speeds)
module_states = kinematics.normalizeWheelSpeeds(module_states, MAX_SPEED)
plot_swerve(wheel_locs, module_states)

Why Trajectory Following?

FRC® games often feature autonomous tasks that require a robot to effectively and accurately move from a known starting location to a known scoring location.

During the 2018 season, Stryke Force changed from the “drive-turn-drive” method to trajectory following.

While more technically challenging, trajectories can be driven much faster since we don’t have to stop to change direction.

Terminology

JSON

Used for PathWeaver trajectory export, JavaScript Object Notation, is an open standard file format, and data interchange format, that uses human-readable text to store and transmit data objects.

Pose

A robot’s pose is the combination of it’s X, Y, and angle (θ) with respect to the field. See the Pose2d class.

Trajectory

A trajectory contains of a collection of State points that represent the pose, curvature, time elapsed, velocity, and acceleration at that point. See the Trajectory class.

Waypoint

Waypoints consists of a X, Y position on the field as well as a robot heading described by a tangent line.

Starting a PathWeaver Project

When starting a new PathWeaver project, we need to provide season-specific information.

Team up with a mentor to set this up for the first time in a season.

Creating a Trajectory

The preferred way to create and visualize a path is with the PathWeaver tool.

PathWeaver Tips and Tricks

• Adding additional waypoints and changing their tangent vectors can affect the path that is followed. Additional waypoints can be added by dragging in the middle of the path.
• Fewer is usually better when it comes to waypoints.
• It’s sometimes hard to precisely set where the waypoints should be placed. In this case, setting the waypoint locations can be done by entering the X and Y value after selecting the waypoint.
• Path Groups are a way of visualizing where one path ends and the next one starts. By adding all the paths to a single path group and selecting the group, all paths in that group will be shown. Each path can be edited while viewing all the paths.

Deploy PathWeaver JSON to Robot

Build all paths in the project by clicking the Build Paths button.

Configure PathWeaver to output JSON files in src/main/deploy/paths. Deploying robot code automatically places JSON files on the roboRIO file system in /home/lvuser/deploy/paths/output.

The PathWeaver JSON can be accessed using getDeployDirectory.

String trajectoryJson = "paths/output/YourPath.wpilib.json";
Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJson);
Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);

Note: Currently PathWeaver displays the coordinate system origin at the top left of the field. When building paths, PathWeaver sets the origin at the bottom left in order to be compatible with the Field2d view in the simulator GUI.

Robot Checklist

There are a few things the robot needs before we can actually drive a trajectory.

1. Swerve modules and motor controllers configured.
   • Motor controllers settings.
   • Azimuth motor closed-loop tuning.
   • Determine the maximum attainable speed for the robot.
   • Measure drive encoder “distance per pulse”.
   • Physically set alignment (or zero) the azimuths.
2. Driver joystick inputs configured.
3. Finish setting up the swerve drive and verify teleop driving. See SwerveDriveKinematics.
4. Determine swerve module drive closed-loop velocity tuning.
5. Configure SwerveDriveOdometry.
6. Configure the HolonomicDriveController. We’ll see this in a later slide.

The HolonomicDriveController Class

The HolonomicDriveController is used to follow trajectories using a holonomic drive train, such as a swerve or mecanum drive.

The holonomic drive controller takes in one PID controller for each direction, forward and strafe, and one profiled PID controller for the yaw.

Because the yaw dynamics are decoupled from translations, we can can specify a custom yaw heading that the robot should point toward. This heading reference is profiled for smoothness to accomodate yaw setpoint changes.

var xController = new PIDController(kPx, kIx, kDx);
var yController = new PIDController(kPy, kIy, kDy);
var thetaController = new ProfiledPIDController(kPt, kIt, kDt, constraints);
var holonomicDriveController = new HolonomicDriveController​(xController yController, thetaController);

Before we dive in to how the holonomic drive controller works, let take a look at a trajectory generated by PathWeaver. Below is a plot of the trajectory states with a some velocity vectors overlayed.

At its most basic, the job of the controller is to convert trajectory states to ChassisSpeeds, which we send to the swerve drive.

In a perfect world, the robot would exactly follow the desired trajectory. In the real world, physical effects such inertia and friction create some error.

The holonomic drive controller compensates for this error accumulation by comparing the robot’s current pose with the desired trajectory state.

The robot’s current pose can be measured in real-time by using SwerveDriveOdometry.

Let’s take a look at the code for following a trajectory using a holonomic drive controller.

We will first reset our robot’s pose to the starting pose of the trajectory. This ensures that the robot’s location on the coordinate system and the trajectory’s starting position are the same.

odometry.resetPose(trajectory.getInitialPose(), gyro.getRotation2d());

It is very important that the initial robot pose match the first pose in the trajectory. Typically this pose is an actual know or measured field position.

If we are using a trajectory that has been defined in robot-centric coordinates, we can first transform it to be relative to the robot’s current pose using the Trajectory transformBy method.

Pose2d initial = trajectory.getInitialPose(); // (1)
Pose2d end = Constants.AUTON_START_POSE;
Transform2d transform = new Transform2d(initial, end);

trajectory = trajectory.transformBy(transform);
odometry.resetPose(trajectory.getInitialPose(), gyro.getRotation2d());

1. Since we created a robot-relative trajectory, the initial pose will be x=0, y=0, θ=0

Assuming our trajectory is being used in a Command, the following code gets executed periodically in the command’s execute() method.

double trajectoryElapsedTime = Timer.get() - trajectoryStartTime;
Trajectory.State state = trajectory.sample(trajectoryElapsedTime); // (1)

odometry.update(gyro.getRotation2d(), getModuleStates()); // (2)

ChassisSpeeds output = holonomicDriveController.calculate(odometry.getPoseMeters(), state, new Rotation2d()); // (3)

drive(output); // (4)

1. Get the desired trajectory state for the current point in time.
2. Update our measured position based on actual gyro reading and swerve module encoders.
3. Calculate the desired v_x, v_y, and ω based on desired trajectory state, corrected for actual measured position.
4. Send the desired speeds to swerve drive kinematics.

Holonomic drive controller error correction

The holonomic drive controller uses an X and Y PIDController to correct position errors while following a trajectory.

The holonomic drive controller uses a ProfiledPIDController to allow us to smoothly change robot yaw while following a trajectory.

Using Constraints

Constraints can limit velocity and acceleration at points in the trajectory based on the current pose, curvature or velocity. The library includes constraints for:

CentripetalAccelerationConstraint

A constraint on the maximum absolute centripetal acceleration allowed when traversing a trajectory. Limiting the maximum centripetal acceleration will cause the robot to slow down around tight turns.

EllipticalRegionConstraint

Enforces a particular constraint only within an elliptical region.

RectangularRegionConstraint

Enforces a particular constraint only within a rectangular region.

SwerveDriveKinematicsConstraint

Enforces constraints on the swerve drive kinematics. This can be used to ensure that the trajectory is constructed so that the commanded velocities for all 4 wheels of the drive train stay below a certain limit.

We apply a trajectory constraint when configuring the TrajectoryConfig.

var kMaxCentripetalAccelerationMetersPerSecondSq = 2;
var centripetalConstraint = new CentripetalAccelerationConstraint(kMaxCentripetalAccelerationMetersPerSecondSq);

var kMaxVelocityMetersPerSecond = 1;
var kMaxAccelerationMetersPerSecondSq = 2;
var trajectoryConfig = new TrajectoryConfig(kMaxVelocityMetersPerSecond, kMaxAccelerationMetersPerSecondSq);
trajectoryConfig.addConstraint​(centripetalConstraint);

A different visualization of the same trajectory.

PathWeaver Path

We can use PathWeaver to create a trajectory for the robot to drive.

PathWeaver Path

PathwWaver internally represents this path as a list of waypoints.

They are saved as CSV-formatted data in *.path files in a PathWeaver project.

PathWeaver Waypoints

The important parts of a waypoint are its coordinates and the length and direction of its tangent vector. We denote the tangent vector with its tail at the waypoint coordinates and its head at tangent x, y.

Waypoint to Trajectory Conversion

This code excerpt from PathWeaver illustrates how it converts a list of waypoints to a trajectory.

private static Trajectory trajectoryFromWaypoints(Iterable<Waypoint> waypoints, TrajectoryConfig config) {
    var list = new TrajectoryGenerator.ControlVectorList();
    for(Waypoint wp: waypoints) {
        list.add(new Spline.ControlVector(
                new double[] {wp.getX(), wp.getTangentX(), 0},
                new double[] {wp.getY(), wp.getTangentY(), 0}));
    }

    return TrajectoryGenerator.generateTrajectory(list, config);
}

But what’s really happening?

Trajectories are Connected Splines

A trajectory is made up of one or more connected splines, each defined by end points and the slope at the end points. The end points of each spline are given by two consecutive waypoints from PathWeaver.

You can connect together as many splines as you need to complete a trajectory.

Here we show the three splines generated from the four PathWeaver waypoints in our earlier example.

Spline Parameterization

The SplineParameterizer (source) class breaks up the spline into various arcs until their dx, dy, and dθ are within specific tolerances.

Each dot on the right plot is a PoseWithCurvature.

We calculate the points for each spline and then append them together to produce the PoseWithCurvature points for the entire trajectory.

At this point, spline parameterization has given us the x, y coordinates and the direction of travel (Pose2d) for each trajectory point.

Trajectory Parameterization

After spline parameterization has broken up our trajectory into manageable segments, we next need to calculate the velocity profile.

The velocity profile is, for each trajectory point, the time that point is reached, and the velocity and acceleration at that point. This calculation is called trajectory parameterization.

This is calculated by the TrajectoryParameterizer class.

When you combine this with the position and travel direction of each trajectory point given by spline parameterization, we have all the information we need for the robot to drive the trajectory. (Spline parameterization also give us curvature at each trajectory point.)

The TrajectoryParameterizer class calculates the final trajectory points.

1. The distance between trajectory point B and the previous point A is calculated.

2. The velocity at B is calculated from the velocity at A, the maximum acceleration, and the distance travelled. \(v_B = \sqrt{v_A^2 +2ad}\)

   Clamp this to the maximum velocity if neccessary.

3. Apply trajectory constraints to maximum acceleration and velocity at point B.

4. Calculate the actual acceleration from the velocity at A, the constrained velocity at B, and the distance travelled. \(a = \frac{v_B^2 - v_A^2}{2d}\)

5. If actual acceleration exceeds the constrained acceleration for B, assign B’s constrained acceleration to A and loop back to start again.

6. If actual acceleration is less than constrained acceleration for B, we are done! Go to next point and repeat.

7. When finished in the forward direction, go through this same entire process backwards through the list of points to make sure we don’t exceed maximum decceleration.

Parameterized Trajectory with Centripetal Constraint

Curvature

A quick detour into terminology — what is curvature?

Curvature is primarily uses to constrain our trajectory velocity and acceleration profile as needed, for example to prevent the robot from tipping in a tight turn.

Field Coordinate System

The field coordinate system (or global coordinate system) is an absolute coordinate system where a point on the field is designated as the origin. Positive θ (theta) is in the counter-clockwise direction, and the positive x-axis points away from your alliance’s driver station wall, and the positive y-axis is perpendicular and to the left of the positive x-axis.

Note: The axes are shown at the middle of the field for visibility. The origins of the coordinate system for each alliance are shown next.

Field Coordinate System Origin

Below is an example of a field coordinate system overlayed on the 2020 FRC field. The red axes shown are for the red alliance, and the blue axes shown are for the blue alliance.

Robot Coordinate System

The robot coordinate system (or local coordinate system) is a relative coordinate system where the robot is the origin. The direction the robot is facing is the positive x axis, and the positive y axis is perpendicular, to the left of the robot. Positive θ is counter-clockwise.

Note: WPILib’s Gyro class is clockwise-positive, so you have to invert the reading in order to get the rotation with either coordinate system.

Translation

Translation in 2 dimensions is represented by WPILib’s Translation2d class. This class has an x and y component, representing the point (x, y).

var pointA = Translation2d(1, 1);
var pointB = Translation2d(3, 1);

System.out.println(pointA.getDistance(pointB)); // 2.0

// vector math
System.out.println(pointB.times(2)) // Translation2d(X: 6.00, Y: 2.00)
System.out.println(pointA.div(2)) // Translation2d(X: 0.50, Y: 0.50)
System.out.println(pointB.unaryMinus()) // Translation2d(X: -3.00, Y: -1.00)

System.out.println(pointA.plus(pointB)); // Translation2d(X: 4.00, Y: 2.00)
System.out.println(pointB.minus(pointA)); // Translation2d(X: 2.00, Y: 0.00)

System.out.println(pointA.rotateBy(Rotation2d.fromDegrees(-45))) // Translation2d(X: 1.41, Y: 0.00)

Rotation

Rotation in 2 dimensions is representated by WPILib’s Rotation2d class. This class has an angle component, which represents the robot’s rotation relative to an axis on a 2-dimensional coordinate system. Positive rotations are counterclockwise.

Rotation2d angleA = Rotation2d.fromDegrees(45);
Rotation2d angleB = Rotation2d.fromDegrees(10);

System.out.println(angleA.plus(angleB)); // Rotation2d(Rads: 0.96, Deg: 55.00)
System.out.println(angleA.rotateBy(angleB)); // Rotation2d(Rads: 0.96, Deg: 55.00)

System.out.println(angleA.minus(angleB)); // Rotation2d(Rads: 0.61, Deg: 35.00)
System.out.println(angleA.rotateBy(angleB.unaryMinus())); // Rotation2d(Rads: 0.61, Deg: 35.00)

System.out.println(angleA.unaryMinus()); // Rotation2d(Rads: -0.79, Deg: -45.00)
System.out.println(angleB.times(2)); // Rotation2d(Rads: 0.35, Deg: 20.00)

Pose

Pose is a combination of both translation and rotation and is represented by the Pose2d class. It can be used to describe the pose of your robot in the field coordinate system, or the pose of objects, such as vision targets, relative to your robot in the robot coordinate system.

  A = Pose2d(
    Translation2d(X: 0.50, Y: 2.00),
    Rotation2d(Rads: 0.00, Deg: 0.00)
  )

  transform = Transform2d(
    Translation2d(X: 2.00, Y: 1.00),
    Rotation2d(Rads: 0.79, Deg: 45.00)
  )

  B = Pose2d(
    Translation2d(X: 2.50, Y: 3.00),
    Rotation2d(Rads: 0.79, Deg: 45.00)
  )