1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <utility>
#include <wpi/SymbolExports.h>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
namespace frc {
class Pose2d;
/**
* Represents a transformation for a Pose2d in the pose's frame.
*/
class WPILIB_DLLEXPORT Transform2d {
public:
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param final The final pose for the transformation.
*/
constexpr Transform2d(const Pose2d& initial, const Pose2d& final);
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
constexpr Transform2d(Translation2d translation, Rotation2d rotation)
: m_translation{std::move(translation)},
m_rotation{std::move(rotation)} {}
/**
* Constructs a transform with x and y translations instead of a separate
* Translation2d.
*
* @param x The x component of the translational component of the transform.
* @param y The y component of the translational component of the transform.
* @param rotation The rotational component of the transform.
*/
constexpr Transform2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
/**
* Constructs a pose with the specified affine transformation matrix.
*
* @param matrix The affine transformation matrix.
* @throws std::domain_error if the affine transformation matrix is invalid.
*/
constexpr explicit Transform2d(const Eigen::Matrix3d& matrix)
: m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}},
m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)},
{matrix(1, 0), matrix(1, 1)}}} {
if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) {
throw std::domain_error("Affine transformation matrix is invalid");
}
}
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
constexpr Transform2d() = default;
/**
* Returns the translation component of the transformation.
*
* @return Reference to the translational component of the transform.
*/
constexpr const Translation2d& Translation() const { return m_translation; }
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
constexpr units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
constexpr units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns an affine transformation matrix representation of this
* transformation.
*/
constexpr Eigen::Matrix3d ToMatrix() const {
auto vec = m_translation.ToVector();
auto mat = m_rotation.ToMatrix();
return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)},
{mat(1, 0), mat(1, 1), vec(1)},
{0.0, 0.0, 1.0}};
}
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
constexpr const Rotation2d& Rotation() const { return m_rotation; }
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
constexpr Transform2d Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
/**
* Multiplies the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform2d.
*/
constexpr Transform2d operator*(double scalar) const {
return Transform2d(m_translation * scalar, m_rotation * scalar);
}
/**
* Divides the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform2d.
*/
constexpr Transform2d operator/(double scalar) const {
return *this * (1.0 / scalar);
}
/**
* Composes two transformations. The second transform is applied relative to
* the orientation of the first.
*
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
*/
constexpr Transform2d operator+(const Transform2d& other) const;
/**
* Checks equality between this Transform2d and another object.
*/
constexpr bool operator==(const Transform2d&) const = default;
private:
Translation2d m_translation;
Rotation2d m_rotation;
};
} // namespace frc
#include "frc/geometry/Pose2d.h"
namespace frc {
constexpr Transform2d::Transform2d(const Pose2d& initial, const Pose2d& final) {
// To transform the global translation delta to be relative to the initial
// pose, rotate by the inverse of the initial pose's orientation.
m_translation = (final.Translation() - initial.Translation())
.RotateBy(-initial.Rotation());
m_rotation = final.Rotation().RelativeTo(initial.Rotation());
}
constexpr Transform2d Transform2d::operator+(const Transform2d& other) const {
return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
}
} // namespace frc
#include "frc/geometry/proto/Transform2dProto.h"
#include "frc/geometry/struct/Transform2dStruct.h"