WPILibC++ 2024.3.2
Kinematics.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <wpi/SymbolExports.h>
8
11
12namespace frc {
13/**
14 * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
15 * into individual wheel speeds. Robot code should not use this directly-
16 * Instead, use the particular type for your drivetrain (e.g.,
17 * DifferentialDriveKinematics).
18 *
19 * Inverse kinematics converts a desired chassis speed into wheel speeds whereas
20 * forward kinematics converts wheel speeds into chassis speed.
21 */
22template <typename WheelSpeeds, typename WheelPositions>
24 public:
25 /**
26 * Performs forward kinematics to return the resulting chassis speed from the
27 * wheel speeds. This method is often used for odometry -- determining the
28 * robot's position on the field using data from the real-world speed of each
29 * wheel on the robot.
30 *
31 * @param wheelSpeeds The speeds of the wheels.
32 * @return The chassis speed.
33 */
35 const WheelSpeeds& wheelSpeeds) const = 0;
36
37 /**
38 * Performs inverse kinematics to return the wheel speeds from a desired
39 * chassis velocity. This method is often used to convert joystick values into
40 * wheel speeds.
41 *
42 * @param chassisSpeeds The desired chassis speed.
43 * @return The wheel speeds.
44 */
45 virtual WheelSpeeds ToWheelSpeeds(
46 const ChassisSpeeds& chassisSpeeds) const = 0;
47
48 /**
49 * Performs forward kinematics to return the resulting Twist2d from the given
50 * change in wheel positions. This method is often used for odometry --
51 * determining the robot's position on the field using changes in the distance
52 * driven by each wheel on the robot.
53 *
54 * @param start The starting distances driven by the wheels.
55 * @param end The ending distances driven by the wheels.
56 *
57 * @return The resulting Twist2d in the robot's movement.
58 */
59 virtual Twist2d ToTwist2d(const WheelPositions& start,
60 const WheelPositions& end) const = 0;
61};
62} // namespace frc
#define WPILIB_DLLEXPORT
Definition: SymbolExports.h:36
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel s...
Definition: Kinematics.h:23
virtual Twist2d ToTwist2d(const WheelPositions &start, const WheelPositions &end) const =0
Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions.
virtual ChassisSpeeds ToChassisSpeeds(const WheelSpeeds &wheelSpeeds) const =0
Performs forward kinematics to return the resulting chassis speed from the wheel speeds.
virtual WheelSpeeds ToWheelSpeeds(const ChassisSpeeds &chassisSpeeds) const =0
Performs inverse kinematics to return the wheel speeds from a desired chassis velocity.
Definition: WheelPositions.h:11
Definition: AprilTagPoseEstimator.h:15
Represents the speed of a robot chassis.
Definition: ChassisSpeeds.h:25
A change in distance along a 2D arc since the last pose update.
Definition: Twist2d.h:21