WPILibC++  2020.3.2
Pose2d.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include "Transform2d.h"
11 #include "Translation2d.h"
12 #include "Twist2d.h"
13 
14 namespace wpi {
15 class json;
16 } // namespace wpi
17 
18 namespace frc {
19 
23 class Pose2d {
24  public:
29  constexpr Pose2d() = default;
30 
37  Pose2d(Translation2d translation, Rotation2d rotation);
38 
47  Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
48 
61  Pose2d operator+(const Transform2d& other) const;
62 
73  Pose2d& operator+=(const Transform2d& other);
74 
81  Transform2d operator-(const Pose2d& other) const;
82 
89  bool operator==(const Pose2d& other) const;
90 
97  bool operator!=(const Pose2d& other) const;
98 
104  const Translation2d& Translation() const { return m_translation; }
105 
111  const Rotation2d& Rotation() const { return m_rotation; }
112 
121  Pose2d TransformBy(const Transform2d& other) const;
122 
135  Pose2d RelativeTo(const Pose2d& other) const;
136 
158  Pose2d Exp(const Twist2d& twist) const;
159 
168  Twist2d Log(const Pose2d& end) const;
169 
170  private:
171  Translation2d m_translation;
172  Rotation2d m_rotation;
173 };
174 
175 void to_json(wpi::json& json, const Pose2d& pose);
176 
177 void from_json(const wpi::json& json, Pose2d& pose);
178 
179 } // namespace frc
wpi::json
a class to store JSON values
Definition: json.h:2655
frc::Pose2d::Pose2d
constexpr Pose2d()=default
Constructs a pose at the origin facing toward the positive X axis.
frc::Pose2d::operator+
Pose2d operator+(const Transform2d &other) const
Transforms the pose by the given transformation and returns the new transformed pose.
frc::Twist2d
A change in distance along arc since the last pose update.
Definition: Twist2d.h:19
frc::Pose2d::operator-
Transform2d operator-(const Pose2d &other) const
Returns the Transform2d that maps the one pose to another.
frc::Pose2d::Translation
const Translation2d & Translation() const
Returns the underlying translation.
Definition: Pose2d.h:104
frc::Pose2d::RelativeTo
Pose2d RelativeTo(const Pose2d &other) const
Returns the other pose relative to the current pose.
frc::Rotation2d
A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:22
frc::Transform2d
Represents a transformation for a Pose2d.
Definition: Transform2d.h:19
frc::Translation2d
Represents a translation in 2d space.
Definition: Translation2d.h:28
frc::Pose2d::Exp
Pose2d Exp(const Twist2d &twist) const
Obtain a new Pose2d from a (constant curvature) velocity.
wpi
WPILib C++ utilities (wpiutil) namespace.
Definition: EventLoopRunner.h:17
frc::Pose2d::TransformBy
Pose2d TransformBy(const Transform2d &other) const
Transforms the pose by the given transformation and returns the new pose.
frc::Pose2d
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
frc::Pose2d::operator!=
bool operator!=(const Pose2d &other) const
Checks inequality between this Pose2d and another object.
frc::Pose2d::Rotation
const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition: Pose2d.h:111
frc::Pose2d::Log
Twist2d Log(const Pose2d &end) const
Returns a Twist2d that maps this pose to the end pose.
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc::Pose2d::operator==
bool operator==(const Pose2d &other) const
Checks equality between this Pose2d and another object.
frc::Pose2d::operator+=
Pose2d & operator+=(const Transform2d &other)
Transforms the current pose by the transformation.