WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
Odometry.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
13
14namespace frc {
15
16/**
17 * Class for odometry. Robot code should not use this directly- Instead, use the
18 * particular type for your drivetrain (e.g., DifferentialDriveOdometry).
19 * Odometry allows you to track the robot's position on the field over a course
20 * of a match using readings from your wheel encoders.
21 *
22 * Teams can use odometry during the autonomous period for complex tasks like
23 * path following. Furthermore, odometry can be used for latency compensation
24 * when using computer-vision systems.
25 *
26 * @tparam WheelSpeeds Wheel speeds type.
27 * @tparam WheelPositions Wheel positions type.
28 */
29template <typename WheelSpeeds, typename WheelPositions>
31 public:
32 /**
33 * Constructs an Odometry object.
34 *
35 * @param kinematics The kinematics for your drivetrain.
36 * @param gyroAngle The angle reported by the gyroscope.
37 * @param wheelPositions The current distances measured by each wheel.
38 * @param initialPose The starting position of the robot on the field.
39 */
41 const Rotation2d& gyroAngle,
42 const WheelPositions& wheelPositions,
43 const Pose2d& initialPose = Pose2d{})
44 : m_kinematics(kinematics),
45 m_pose(initialPose),
46 m_previousWheelPositions(wheelPositions) {
47 m_previousAngle = m_pose.Rotation();
48 m_gyroOffset = m_pose.Rotation() - gyroAngle;
49 }
50
51 /**
52 * Resets the robot's position on the field.
53 *
54 * The gyroscope angle does not need to be reset here on the user's robot
55 * code. The library automatically takes care of offsetting the gyro angle.
56 *
57 * @param gyroAngle The angle reported by the gyroscope.
58 * @param wheelPositions The current distances measured by each wheel.
59 * @param pose The position on the field that your robot is at.
60 */
61 void ResetPosition(const Rotation2d& gyroAngle,
62 const WheelPositions& wheelPositions, const Pose2d& pose) {
63 m_pose = pose;
64 m_previousAngle = pose.Rotation();
65 m_gyroOffset = m_pose.Rotation() - gyroAngle;
66 m_previousWheelPositions = wheelPositions;
67 }
68
69 /**
70 * Resets the pose.
71 *
72 * @param pose The pose to reset to.
73 */
74 void ResetPose(const Pose2d& pose) {
75 m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation());
76 m_pose = pose;
77 m_previousAngle = pose.Rotation();
78 }
79
80 /**
81 * Resets the translation of the pose.
82 *
83 * @param translation The translation to reset to.
84 */
85 void ResetTranslation(const Translation2d& translation) {
86 m_pose = Pose2d{translation, m_pose.Rotation()};
87 }
88
89 /**
90 * Resets the rotation of the pose.
91 *
92 * @param rotation The rotation to reset to.
93 */
94 void ResetRotation(const Rotation2d& rotation) {
95 m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation());
96 m_pose = Pose2d{m_pose.Translation(), rotation};
97 m_previousAngle = rotation;
98 }
99
100 /**
101 * Returns the position of the robot on the field.
102 * @return The pose of the robot.
103 */
104 const Pose2d& GetPose() const { return m_pose; }
105
106 /**
107 * Updates the robot's position on the field using forward kinematics and
108 * integration of the pose over time. This method takes in an angle parameter
109 * which is used instead of the angular rate that is calculated from forward
110 * kinematics, in addition to the current distance measurement at each wheel.
111 *
112 * @param gyroAngle The angle reported by the gyroscope.
113 * @param wheelPositions The current distances measured by each wheel.
114 *
115 * @return The new pose of the robot.
116 */
117 const Pose2d& Update(const Rotation2d& gyroAngle,
118 const WheelPositions& wheelPositions) {
119 auto angle = gyroAngle + m_gyroOffset;
120
121 auto twist =
122 m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
123 twist.dtheta = (angle - m_previousAngle).Radians();
124
125 auto newPose = m_pose.Exp(twist);
126
127 m_previousAngle = angle;
128 m_previousWheelPositions = wheelPositions;
129 m_pose = {newPose.Translation(), angle};
130
131 return m_pose;
132 }
133
134 private:
135 const Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
136 Pose2d m_pose;
137
138 WheelPositions m_previousWheelPositions;
139 Rotation2d m_previousAngle;
140 Rotation2d m_gyroOffset;
141};
142
143} // 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:25
Class for odometry.
Definition Odometry.h:30
Odometry(const Kinematics< WheelSpeeds, WheelPositions > &kinematics, const Rotation2d &gyroAngle, const WheelPositions &wheelPositions, const Pose2d &initialPose=Pose2d{})
Constructs an Odometry object.
Definition Odometry.h:40
void ResetRotation(const Rotation2d &rotation)
Resets the rotation of the pose.
Definition Odometry.h:94
const Pose2d & Update(const Rotation2d &gyroAngle, const WheelPositions &wheelPositions)
Updates the robot's position on the field using forward kinematics and integration of the pose over t...
Definition Odometry.h:117
void ResetPosition(const Rotation2d &gyroAngle, const WheelPositions &wheelPositions, const Pose2d &pose)
Resets the robot's position on the field.
Definition Odometry.h:61
void ResetTranslation(const Translation2d &translation)
Resets the translation of the pose.
Definition Odometry.h:85
const Pose2d & GetPose() const
Returns the position of the robot on the field.
Definition Odometry.h:104
void ResetPose(const Pose2d &pose)
Resets the pose.
Definition Odometry.h:74
Represents a 2D pose containing translational and rotational elements.
Definition Pose2d.h:28
constexpr const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition Pose2d.h:128
A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Definition Rotation2d.h:31
Represents a translation in 2D space.
Definition Translation2d.h:29
Definition CAN.h:11