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
// 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 <wpi/SymbolExports.h>
#include "frc/geometry/CoordinateAxis.h"
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Rotation3d.h"
#include "frc/geometry/Translation3d.h"
namespace frc {
/**
* A helper class that converts Pose3d objects between different standard
* coordinate frames.
*/
class WPILIB_DLLEXPORT CoordinateSystem {
public:
/**
* Constructs a coordinate system with the given cardinal directions for each
* axis.
*
* @param positiveX The cardinal direction of the positive x-axis.
* @param positiveY The cardinal direction of the positive y-axis.
* @param positiveZ The cardinal direction of the positive z-axis.
* @throws std::domain_error if the coordinate system isn't special orthogonal
*/
constexpr CoordinateSystem(const CoordinateAxis& positiveX,
const CoordinateAxis& positiveY,
const CoordinateAxis& positiveZ) {
// Construct a change of basis matrix from the source coordinate system to
// the NWU coordinate system. Each column vector in the change of basis
// matrix is one of the old basis vectors mapped to its representation in
// the new basis.
Eigen::Matrix3d R{
{positiveX.m_axis(0), positiveY.m_axis(0), positiveZ.m_axis(0)},
{positiveX.m_axis(1), positiveY.m_axis(1), positiveZ.m_axis(1)},
{positiveX.m_axis(2), positiveY.m_axis(2), positiveZ.m_axis(2)}};
// The change of basis matrix should be a pure rotation. The Rotation3d
// constructor will verify this by checking for special orthogonality.
m_rotation = Rotation3d{R};
}
/**
* Returns an instance of the North-West-Up (NWU) coordinate system.
*
* The +X axis is north, the +Y axis is west, and the +Z axis is up.
*/
constexpr static CoordinateSystem NWU() {
return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::W(),
CoordinateAxis::U()};
}
/**
* Returns an instance of the East-Down-North (EDN) coordinate system.
*
* The +X axis is east, the +Y axis is down, and the +Z axis is north.
*/
constexpr static CoordinateSystem EDN() {
return CoordinateSystem{CoordinateAxis::E(), CoordinateAxis::D(),
CoordinateAxis::N()};
}
/**
* Returns an instance of the NED coordinate system.
*
* The +X axis is north, the +Y axis is east, and the +Z axis is down.
*/
constexpr static CoordinateSystem NED() {
return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::E(),
CoordinateAxis::D()};
}
/**
* Converts the given translation from one coordinate system to another.
*
* @param translation The translation to convert.
* @param from The coordinate system the translation starts in.
* @param to The coordinate system to which to convert.
* @return The given translation in the desired coordinate system.
*/
constexpr static Translation3d Convert(const Translation3d& translation,
const CoordinateSystem& from,
const CoordinateSystem& to) {
// Convert to NWU, then convert to the new coordinate system
return translation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
}
/**
* Converts the given rotation from one coordinate system to another.
*
* @param rotation The rotation to convert.
* @param from The coordinate system the rotation starts in.
* @param to The coordinate system to which to convert.
* @return The given rotation in the desired coordinate system.
*/
constexpr static Rotation3d Convert(const Rotation3d& rotation,
const CoordinateSystem& from,
const CoordinateSystem& to) {
// Convert to NWU, then convert to the new coordinate system
return rotation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
}
/**
* Converts the given pose from one coordinate system to another.
*
* @param pose The pose to convert.
* @param from The coordinate system the pose starts in.
* @param to The coordinate system to which to convert.
* @return The given pose in the desired coordinate system.
*/
constexpr static Pose3d Convert(const Pose3d& pose,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return Pose3d{Convert(pose.Translation(), from, to),
Convert(pose.Rotation(), from, to)};
}
/**
* Converts the given transform from one coordinate system to another.
*
* @param transform The transform to convert.
* @param from The coordinate system the transform starts in.
* @param to The coordinate system to which to convert.
* @return The given transform in the desired coordinate system.
*/
constexpr static Transform3d Convert(const Transform3d& transform,
const CoordinateSystem& from,
const CoordinateSystem& to) {
// coordRot is the rotation that converts between the coordinate systems
// when applied extrinsically. It first converts to NWU, then converts to
// the new coordinate system.
const auto coordRot = from.m_rotation.RotateBy(-to.m_rotation);
// The new rotation is the extrinsic rotation from convert(zero) to
// convert(transformRot). That is, applying convertedRot extrinsically to
// convert(zero) produces convert(transformRot). In the below snippet, we
// use matrix notation, so rotA rotB applies rotA extrinsically to rotB.
//
// convertedRot convert(zero) = convert(transformRot)
// convertedRot = convert(transformRot) convert(zero)⁻¹
// = (coordRot transformRot) (coordRot zero)⁻¹
// = (coordRot transformRot) coordRot⁻¹
//
// In code, the equivalent for rotA rotB is rotB.RotateBy(rotA) (note the
// change in order), and the equivalent for rot⁻¹ is -rot.
return Transform3d{
Convert(transform.Translation(), from, to),
(-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
}
private:
// Rotation from this coordinate system to NWU coordinate system when applied
// extrinsically
Rotation3d m_rotation;
};
} // namespace frc